diff --git a/.gitignore b/.gitignore index 1aa19e47..bbc66bdc 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,11 @@ # This gitignore has been specially created by the WPILib team. # If you remove items from this file, intellisense might break. +### Sim ### + +ctre_sim +./ctre_sim/ + ### C++ ### # Prerequisites *.d diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json new file mode 100644 index 00000000..5cf1fe6a --- /dev/null +++ b/.pathplanner/settings.json @@ -0,0 +1,12 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 1.0, + "defaultMaxAccel": 1.5, + "defaultMaxAngVel": 100.0, + "defaultMaxAngAccel": 150.0, + "maxModuleSpeed": 1.0 +} diff --git a/Autos/Taxi b/Autos/Taxi new file mode 100644 index 00000000..8b137891 --- /dev/null +++ b/Autos/Taxi @@ -0,0 +1 @@ + diff --git a/Paths/taxi.path b/Paths/taxi.path new file mode 100644 index 00000000..176b2e82 --- /dev/null +++ b/Paths/taxi.path @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name +5.506651295445286,-7.900861866896187,0.0,0.0,true,false, +6.5729,-13.5677075,0.0,-13.5677075,true,false, diff --git a/build.gradle b/build.gradle index a525d62b..6524664b 100644 --- a/build.gradle +++ b/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2024.3.1" + id "edu.wpi.first.GradleRIO" version "2024.2.1" } // Define my targets (RoboRIO) and artifacts (deployable files) @@ -65,6 +65,29 @@ model { } binaries.all { + if (!project.hasProperty("opt")) { + def opt; + if (opt == null) { + opt = "release" + } + + if (!opt == "release") { + cppCompiler.define "DEBUG" + } else { + cppCompiler.define "RELEASE" + } + } else { + if (opt == null) { + opt = "release" + } + + if (!opt == "release") { + cppCompiler.define "DEBUG" + } else { + cppCompiler.define "RELEASE" + } + } + lib project: ':wombat', library: 'Wombat', linkage: 'static' } @@ -93,6 +116,29 @@ model { } binaries.all { + if (!project.hasProperty("opt")) { + def opt; + if (opt == null) { + opt = "release" + } + + if (!opt == "release") { + cppCompiler.define "DEBUG" + } else { + cppCompiler.define "RELEASE" + } + } else { + if (opt == null) { + opt = "release" + } + + if (!opt == "release") { + cppCompiler.define "DEBUG" + } else { + cppCompiler.define "RELEASE" + } + } + lib project: ':wombat', library: 'Wombat', linkage: 'static' } diff --git a/gradlew.bat b/gradlew.bat index 25da30db..d77d7b88 100644 --- a/gradlew.bat +++ b/gradlew.bat @@ -8,6 +8,7 @@ @rem https://www.apache.org/licenses/LICENSE-2.0 @rem @rem Unless required by applicable law or agreed to in writing, software + @rem distributed under the License is distributed on an "AS IS" BASIS, @rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. @rem See the License for the specific language governing permissions and @@ -22,7 +23,7 @@ @rem ########################################################################## @rem Set local scope for the variables with windows NT shell -if "%OS%"=="Windows_NT" setlocal +if "%OS%"=="Windows_NT" setlocal set DIRNAME=%~dp0 if "%DIRNAME%"=="" set DIRNAME=. diff --git a/networktables.json b/networktables.json new file mode 100644 index 00000000..fe51488c --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/pathweaver.json b/pathweaver.json new file mode 100644 index 00000000..1c8f1fd3 --- /dev/null +++ b/pathweaver.json @@ -0,0 +1,8 @@ +{ + "lengthUnit": "FOOT", + "exportUnit": "Always Meters", + "maxVelocity": 10.0, + "maxAcceleration": 60.0, + "trackWidth": 2.0, + "gameName": "Crescendo" +} diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 00000000..73cc713c --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 00000000..2c54b6e6 --- /dev/null +++ b/simgui.json @@ -0,0 +1,45 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Auto Selector": "String Chooser", + "/SmartDashboard/Field": "Field2d" + }, + "windows": { + "/SmartDashboard/Auto Selector": { + "window": { + "visible": true + } + }, + "/SmartDashboard/Field": { + "bottom": 1476, + "height": 8.210550308227539, + "left": 150, + "right": 2961, + "top": 79, + "width": 16.541748046875, + "window": { + "visible": true + } + } + } + }, + "NetworkTables": { + "transitory": { + "drivetrain": { + "pid": { + "open": true + } + }, + "pathplanner": { + "open": true + }, + "vision": { + "open": true + } + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp index 3d7f72f8..b08bf4cd 100644 --- a/src/main/cpp/AlphaArm.cpp +++ b/src/main/cpp/AlphaArm.cpp @@ -4,89 +4,129 @@ #include "AlphaArm.h" - -AlphaArm::AlphaArm(AlphaArmConfig *config/*, frc::Rotation2d initialAngle, wom::vision::Limelight* vision */) : _config(config), _pidArm{frc::PIDController(1.2, 0.4, 0)}, _pidArmStates{frc::PIDController(37, 0.00070, 0.15)}, _pidIntakeState{frc::PIDController(30, 0.00015, 0.005)} -{ - -} - -void AlphaArm::OnStart(){ +AlphaArm::AlphaArm(AlphaArmConfig* config /*, frc::Rotation2d initialAngle, wom::vision::Limelight* vision */) + : _config(config), + _pidArm{frc::PIDController(1.2, 0.4, 0)}, + _pidArmStates{frc::PIDController(37, 0.00070, 0.15)}, + _pidIntakeState{frc::PIDController(30, 0.00015, 0.005)}, + _pidClimberStates{frc::PIDController(25, 0.00015, 0.005)} {} + +void AlphaArm::OnStart() { _pidArmStates.Reset(); _pidIntakeState.Reset(); - } void AlphaArm::OnUpdate(units::second_t dt) { switch (_state) { case AlphaArmState::kIdle: - + _setAlphaArmVoltage = 0_V; break; case AlphaArmState::kRaw: - std::cout << "RawControl" << std::endl; - _setAlphaArmVoltage = _rawArmVoltage; - + #ifdef DEBUG + std::cout << "RawControl" << std::endl; + #endif + _setAlphaArmVoltage = _rawArmVoltage; + break; - case AlphaArmState::kHoldAngle: - { - _pidArm.SetSetpoint(-_goal); - //_setAlphaArmVoltage = _pidArm.Calculate(_alphaArm->GetConfig().config->alphaArmEncoder.GetEncoderPosition()); - _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))}; - } - break; + case AlphaArmState::kHoldAngle: { + _pidIntakeState.SetSetpoint(-_goal); + //_setAlphaArmVoltage = + //_pidArm.Calculate(_alphaArm->GetConfig().config->alphaArmEncoder.GetEncoderPosition()); + _setAlphaArmVoltage = units::volt_t{ + _pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))}; + } break; case AlphaArmState::kAmpAngle: + #ifdef DEBUG std::cout << "Amp Angle" << std::endl; - - _pidArmStates.SetSetpoint(-2.17); - _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)) )}; + #endif - break; + _pidArmStates.SetSetpoint(-2.12); + _setAlphaArmVoltage = units::volt_t{ + _pidArmStates.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))}; + + break; case AlphaArmState::kIntakeAngle: - std::cout << "Intake Angle" << std::endl; - _pidIntakeState.SetSetpoint(-0.48); //-0.48 - _setAlphaArmVoltage = units::volt_t{_pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))}; - break; + #ifdef DEBUG + std::cout << "Intake Angle" << std::endl; + #endif + _pidIntakeState.SetSetpoint(-0.50); //-0.48 + _setAlphaArmVoltage = units::volt_t{ + _pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))}; + break; + case AlphaArmState::kIntakedAngle: + #ifdef DEBUG + std::cout << "Intake Angle" << std::endl; + #endif + _pidIntakeState.SetSetpoint(-0.55); //-0.48 + _setAlphaArmVoltage = units::volt_t{ + _pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))}; + break; + case AlphaArmState::kClimbAngle: + #ifdef DEBUG + std::cout << "Climb Angle" << std::endl; + #endif + _pidArmStates.SetSetpoint(-2.00); //-0.48 + _setAlphaArmVoltage = units::volt_t{ + _pidArmStates.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))}; + break; + + case AlphaArmState::kClimbed: + #ifdef DEBUG + std::cout << "Climb Angle" << std::endl; + #endif + _pidClimberStates.SetSetpoint(-0.5); //-0.48 + _setAlphaArmVoltage = units::volt_t{_pidClimberStates.Calculate( + -(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))}; + break; case AlphaArmState::kSpeakerAngle: - std::cout << "Speaker Angle" << std::endl; - _pidArmStates.SetSetpoint(-0.82); - _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))}; + #ifdef DEBUG + std::cout << "Speaker Angle" << std::endl; + #endif + _pidIntakeState.SetSetpoint(-0.82); + _setAlphaArmVoltage = units::volt_t{ + _pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))}; - break; + break; - // case AlphaArmState::kVisionAngle: - // std::cout << "Vision Angle" << std::endl; + // case AlphaArmState::kVisionAngle: + // std::cout << "Vision Angle" << std::endl; - // break; + // break; case AlphaArmState::kStowed: - std::cout << "Stowed" << std::endl; - _pidArmStates.SetSetpoint(-0.52); - _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))}; - default: + #ifdef DEBUG + std::cout << "Shuttling angle" << std::endl; + #endif + _pidIntakeState.SetSetpoint(-1.20); + _setAlphaArmVoltage = units::volt_t{ + _pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))}; + default: std::cout << "Error: alphaArm in INVALID STATE" << std::endl; break; - } - _config->alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage); - _config->alphaArmGearbox2.motorController->SetVoltage(_setAlphaArmVoltage); - std::cout << "Encoder Value: " << (_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)) << std::endl; - _table->GetEntry("PID Error").SetDouble(_pidArm.GetPositionError()); - _table->GetEntry("SetPoint").SetDouble(_pidArm.GetSetpoint()); - _table->GetEntry("Input").SetDouble((_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415))); - - _table->GetEntry("PID Error State").SetDouble(_pidArmStates.GetPositionError()); - _table->GetEntry("SetPoint State").SetDouble(_pidArmStates.GetSetpoint()); - - _table->GetEntry("Intake SetPoint State").SetDouble(_pidIntakeState.GetSetpoint()); - _table->GetEntry("Intake PID Error State").SetDouble(_pidIntakeState.GetPositionError()); - - - - std::cout << "Voltage:" << _setAlphaArmVoltage.value() << std::endl; - + _config->alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage); + _config->alphaArmGearbox2.motorController->SetVoltage(_setAlphaArmVoltage); + #ifdef DEBUG + std::cout << "Encoder Value: " << (_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)) + << std::endl; + #endif + _table->GetEntry("PID Error").SetDouble(_pidArm.GetPositionError()); + _table->GetEntry("SetPoint").SetDouble(_pidArm.GetSetpoint()); + _table->GetEntry("Input").SetDouble((_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415))); + + _table->GetEntry("PID Error State").SetDouble(_pidArmStates.GetPositionError()); + _table->GetEntry("SetPoint State").SetDouble(_pidArmStates.GetSetpoint()); + + _table->GetEntry("Intake SetPoint State").SetDouble(_pidIntakeState.GetSetpoint()); + _table->GetEntry("Intake PID Error State").SetDouble(_pidIntakeState.GetPositionError()); + + #ifdef DEBUG + std::cout << "Voltage:" << _setAlphaArmVoltage.value() << std::endl; + #endif } void AlphaArm::SetState(AlphaArmState state) { @@ -97,13 +137,10 @@ void AlphaArm::SetArmRaw(units::volt_t voltage) { _rawArmVoltage = voltage; } - -void AlphaArm::SetGoal(double goal){ +void AlphaArm::SetGoal(double goal) { _goal = goal; } -void AlphaArm::SetControllerRaw(units::volt_t voltage){ +void AlphaArm::SetControllerRaw(units::volt_t voltage) { _controlledRawVoltage = voltage; } - - diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp index cee72cb6..978dba07 100644 --- a/src/main/cpp/AlphaArmBehaviour.cpp +++ b/src/main/cpp/AlphaArmBehaviour.cpp @@ -6,8 +6,11 @@ #include -AlphaArmManualControl::AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver) - : _alphaArm(alphaArm), _codriver(codriver) { +#include "vision/Vision.h" + +AlphaArmManualControl::AlphaArmManualControl(AlphaArm* alphaArm, Intake* intake, + frc::XboxController* codriver) + : _alphaArm(alphaArm), _intake(intake), _codriver(codriver) { Controls(alphaArm); } @@ -16,12 +19,12 @@ AlphaArmConfig AlphaArm::GetConfig() { } void AlphaArmManualControl::OnTick(units::second_t dt) { - + #ifdef DEBUG _table->GetEntry("State").SetBoolean(_rawControl); _table->GetEntry("Goal Value").SetBoolean(_gotValue); - + #endif - if (_codriver->GetBButton()) { + if (_codriver->GetBackButton()) { if (_rawControl == true) { _rawControl = false; } else { @@ -32,26 +35,35 @@ void AlphaArmManualControl::OnTick(units::second_t dt) { if (_rawControl) { _alphaArm->SetState(AlphaArmState::kRaw); if (wom::deadzone(_codriver->GetRightY())) { - _alphaArm->SetArmRaw(_codriver->GetRightY() * 8_V); + _alphaArm->SetArmRaw(_codriver->GetRightY() * 7_V); } else { _alphaArm->SetArmRaw(0_V); } } else { - if(_codriver->GetLeftTriggerAxis() > 0.1){ - _alphaArm->SetState(AlphaArmState::kSpeakerAngle); - } else if (_codriver->GetLeftBumper()){ - _alphaArm->SetState(AlphaArmState::kAmpAngle); - } else if(_codriver->GetYButton()){ - _alphaArm->SetState(AlphaArmState::kStowed); - } else if(_codriver->GetRightBumper()){ - _alphaArm->SetState(AlphaArmState::kIntakeAngle); + _table->GetEntry("CLIMBING:").SetBoolean(climbing); + if (_codriver->GetPOV() == 90 || _codriver->GetPOV() == 180 || _codriver->GetPOV() == 270) { + climbing = true; + } + if (_codriver->GetPOV() == 0) { + climbing = false; } else { - _alphaArm->SetState(AlphaArmState::kIdle); + if (!climbing) { + if (_codriver->GetLeftTriggerAxis() > 0.1) { + _alphaArm->SetState(AlphaArmState::kIntakeAngle); + } else if (_codriver->GetLeftBumper()) { + _alphaArm->SetState(AlphaArmState::kAmpAngle); + } else if (_codriver->GetAButton()) { + _alphaArm->SetState(AlphaArmState::kStowed); + } else if (_codriver->GetPOV() == 90) { + _alphaArm->SetState(AlphaArmState::kClimbAngle); + } else { + if (_intake->GetState() == IntakeState::kHold) { + _alphaArm->SetState(AlphaArmState::kIntakedAngle); + } else { + _alphaArm->SetState(AlphaArmState::kIntakeAngle); + } + } + } } } - } - - - - diff --git a/src/main/cpp/Auto.cpp b/src/main/cpp/Auto.cpp new file mode 100644 index 00000000..b10693d6 --- /dev/null +++ b/src/main/cpp/Auto.cpp @@ -0,0 +1,327 @@ +// Copyright (c) 2023-2024 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + +#include "Auto.h" + +#include + +#include "AlphaArmBehaviour.h" +#include "IntakeBehaviour.h" +#include "ShooterBehaviour.h" +#include "behaviour/Behaviour.h" +#include "drivetrain/behaviours/SwerveBehaviours.h" +#include "frc/geometry/Pose2d.h" +#include "units/angle.h" +#include "utils/Pathplanner.h" + +// Shoots starting note then moves out of starting position. +wom::SwerveAutoBuilder* autos::InitCommands(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, + Intake* _intake, AlphaArm* _alphaArm) { + wom::AutoCommands c = *new wom::AutoCommands( + {// {"ArmToSetPoint", [_alphaArm]() { return wom::make(_alphaArm, 20_deg); }}, + // {"Shoot", [_shooter]() { return wom::make(_shooter); }}, + {"IntakeNote", + [_intake]() { + return wom::make(_intake) /*->Until(wom::make(3_s))*/; + }}, + {"PassNote", + [_intake]() { return wom::make(_intake)->Until(wom::make(1_s)); }}, + {"EjectNote", [_intake]() { return wom::make(_intake)->WithTimeout(1_s); }}, + {"Shoot", [_shooter, _intake]() { + return wom::make(_shooter, _intake, 300_rad_per_s) + ->Until(wom::make(2_s)); + }}, + {"TurnToThirdNote", [_swerveDrive]() { return wom::make(_swerveDrive, -50_deg); }}, + {"TurnToSpeakerFromThird", [_swerveDrive]() { return wom::make(_swerveDrive, 15_deg); }}, + {"TurnToSecondNote", [_swerveDrive]() { return wom::make(_swerveDrive, 30_deg); }}, + {"TurnToSpeakerFromSecond", [_swerveDrive]() { return wom::make(_swerveDrive, -10_deg); }} + }); + + return new wom::utils::SwerveAutoBuilder(_swerveDrive, "Taxi", c); +} + +std::shared_ptr autos::Taxi(wom::SwerveAutoBuilder* builder) { + return builder->GetAutoRoutine("Taxi"); + // return behaviour::make(_alphaArm, 0_deg); + // behaviour::make(_shooter); + // behaviour::make(_alphaArm, 1_deg); +} + +std::shared_ptr autos::OneNoteTaxi(wom::SwerveAutoBuilder* builder) { + return builder->GetAutoRoutine("OneNoteTaxi"); +} + +std::shared_ptr autos::FourNoteTaxi(wom::SwerveAutoBuilder* builder) { + return builder->GetAutoRoutine("FourNoteTaxiBad"); +} + +std::shared_ptr autos::ThreeNoteTaxi(wom::SwerveAutoBuilder* builder) { + return builder->GetAutoRoutine("ThreeNoteTaxiBad"); +} + +// std::shared_ptr autos::QuadrupleClose(wom::drivetrain::SwerveDrive* _swerveDrive, +// Shooter* _shooter, Intake* _intake, +// AlphaArm* _alphaArm) { +// return behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_shooter); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_intake); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_shooter); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_intake); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_shooter); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_intake); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_shooter); +// +// /* +// 4N Close +// 1. Shoot starting note into speaker +// 2. Intake note from close note +// 3. Shoot note into speaker +// 4. Intake note from close floor note +// 5. Shoot note into speaker +// 6. Intake not from close floor +// 7. Shoot note +// */ +// } +// +// std::shared_ptr autos::QuadrupleFar(wom::drivetrain::SwerveDrive* _swerveDrive, +// Shooter* _shooter, Intake* _intake, +// AlphaArm* _alphaArm) { +// return behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_shooter); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_intake); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_shooter); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_intake); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_shooter); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_intake); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_shooter); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_intake); +// +// /* +// 4N Far +// 1. Shoot start note in speaker +// 2. Drive to far note +// 3. Intake note +// 4. Drive back to shooting line +// 5. Shoot note into speaker +// 6. Drive to note +// 7. Intake note +// 8. Drive to shooting line +// 9. Shoot note +// 10. Drive to note +// 11. Intake note +// 12. Drive to shooting line +// 13. Shoot note +// 14. Drive to intake note (if possible) +// */ +// } +// +// std::shared_ptr autos::QuadrupleCloseDoubleFar( +// wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) { +// return behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_shooter); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_intake); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_shooter); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_intake); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_shooter); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_intake); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_shooter); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_intake); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_shooter); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_intake); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_shooter); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_intake); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_shooter); +// +// // 4N Close 2N Far +// // 1. Shoot note +// // 2. Drive to close note +// // 3. Intake note +// // 4. Shoot note +// // 5. Drive to close note +// // 6. Intake note +// // 7. Shoot note +// // 8. Drive to close note +// // 9. Intake note +// // 10. Shoot note +// // 11. Drive to far note +// // 12. Intake note +// // 13. Drive to shooting line +// // 14. Shoot note +// // 15. Drive to far note +// // 16. Intake note +// // 17. Drive to shooting line +// // 18. Shoot note +// } +// +// std::shared_ptr autos::QuadrupleCloseSingleFar( +// wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) { +// return behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_shooter); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_intake); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_shooter); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_intake); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_shooter); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_intake); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_alphaArm, 1_deg); +// behaviour::make(_shooter); +// behaviour::make(_alphaArm, 1_deg); +// } +// +// // 4N Close 1N Far +// // 1. Shoot note +// // 2. Drive to close note +// // 3. Intake note +// // 4. Drive to speaker +// // 5. Shoot note +// // 6. Drive to close note +// // 7. Intake note +// // 8. Drive to speaker +// // 9. Shoot note +// // 10. Drive to close note +// // 11. Intake note +// // 12. Drive to speaker +// // 13. Shoot note +// // 14. Drive to far note +// // 15. Intake note +// // 15. Drive to speaker +// // 16. shoot +// +// // /* +// // TRAP AUTO +// // 1. Drive to trap +// // 2. Climb up +// // 3. Shoot note +// // 4. Climb down +// // 5. Drive to far note +// // 6. Intake note +// // 7. Drive to trap +// // 8. Climb up +// // 9. Shoot note +// // 10. Climb down +// // 11. Drive to far note +// // 12. Intake +// // 13. Drive to trap +// // 14. Climb up +// // 15. Shoot note +// // 16. Climb down +// // */ +// // } +// +// std::shared_ptr autos::AutoTest(wom::drivetrain::SwerveDrive* _swerveDrive, +// Shooter* _shooter, Intake* _intake, +// AlphaArm* _alphaArm) { +// return behaviour::make(_alphaArm, 1_deg); +// behaviour::make( +// _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false); +// behaviour::make(_shooter); +// behaviour::make(_intake); +// } // This auto is a test for auto to see if all things work. diff --git a/src/main/cpp/Climber.cpp b/src/main/cpp/Climber.cpp new file mode 100644 index 00000000..1fdea0d0 --- /dev/null +++ b/src/main/cpp/Climber.cpp @@ -0,0 +1,83 @@ +// Copyright (c) 2023-2024 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + +#include "Climber.h" + +Climber::Climber(ClimberConfig config) : _config(config), _pid(frc::PIDController(0.35, 0, 0, 0.05_s)) {} + +void Climber::OnUpdate(units::second_t dt) { + switch (_state) { + case ClimberState::kIdle: { + _stringStateName = "Idle"; + _setVoltage = 0_V; + } break; + + case ClimberState::kRatchet: { + _stringStateName = "Ratchet"; + _setVoltage = 0_V; + // armServo.SetAngle(0); + } break; + + case ClimberState::kArmUp: { + _stringStateName = "Arm Up"; + // _setVoltage = 8_V; + + _pid.SetSetpoint(0.035 * 100); + _setVoltage = -units::volt_t{ + _pid.Calculate((-_config.climberGearbox.encoder->GetEncoderPosition().value()) * 100)}; + #ifdef DEBUG + _table->GetEntry("PID OUTPUT") + .SetDouble(_pid.Calculate((-_config.climberGearbox.encoder->GetEncoderPosition().value()) * 100)); + #endif + } break; + + case ClimberState::kArmDown: { + _stringStateName = "Arm Down"; + _pid.SetSetpoint(0.07 * 100); + _setVoltage = -units::volt_t{ + _pid.Calculate((-_config.climberGearbox.encoder->GetEncoderPosition().value()) * 100)}; + // armServo.SetAngle(180); + // _setVoltage = -8_V; + } break; + + case ClimberState::kMatch: { + _stringStateName = "Match"; + _pid.SetSetpoint(0.005 * 100); + _setVoltage = -units::volt_t{ + _pid.Calculate((-_config.climberGearbox.encoder->GetEncoderPosition().value()) * 100)}; + + // _pid.SetSetpoint() + // _setVoltage = -8_V; + } break; + + case ClimberState::kRaw: { + _stringStateName = "Raw"; + _setVoltage = _rawVoltage; + } break; + + default: + std::cout << "Error climber in invalid state" << std::endl; + break; + } + _config.climberGearbox.motorController->SetVoltage(_setVoltage); + + #ifdef DEBUG + _table->GetEntry("State: ").SetString(_stringStateName); + _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value()); + _table->GetEntry("Encoder Output: ") + .SetDouble(_config.climberGearbox.encoder->GetEncoderPosition().value() * 100); + #endif +} + +void Climber::SetState(ClimberState state) { + _state = state; +} + +void Climber::SetRaw(units::volt_t voltage) { + _rawVoltage = voltage; +} + +ClimberState Climber::GetState() { + return _state; +} diff --git a/src/main/cpp/ClimberBehaviour.cpp b/src/main/cpp/ClimberBehaviour.cpp new file mode 100644 index 00000000..113756c5 --- /dev/null +++ b/src/main/cpp/ClimberBehaviour.cpp @@ -0,0 +1,39 @@ +// Copyright (c) 2023-2024 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + +#include "ClimberBehaviour.h" + +ClimberManualControl::ClimberManualControl(Climber* climber, AlphaArm* arm, frc::XboxController* codriver) + : _climber(climber), _arm(arm), _codriver(codriver) { + Controls(climber); +} + +void ClimberManualControl::OnTick(units::second_t dt) { + if (_codriver->GetAButtonPressed()) { + if (_rawControl) { + _rawControl = false; + } else { + _rawControl = true; + } + } + + if (_rawControl) { + _climber->SetState(ClimberState::kRaw); + if (wom::deadzone(_codriver->GetLeftY())) { + _climber->SetRaw(_codriver->GetLeftY() * 6_V); + } else { + _climber->SetRaw(0_V); + } + } else { + if (_codriver->GetPOV() == 90) { + _climber->SetState(ClimberState::kMatch); + _arm->SetState(AlphaArmState::kClimbAngle); + } else if (_codriver->GetPOV() == 180) { + _climber->SetState(ClimberState::kArmDown); + } else if (_codriver->GetPOV() == 270) { + _climber->SetState(ClimberState::kRatchet); + _arm->SetState(AlphaArmState::kClimbed); + } + } +} diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp index 6110aa7e..113b9b22 100644 --- a/src/main/cpp/Intake.cpp +++ b/src/main/cpp/Intake.cpp @@ -4,78 +4,133 @@ #include "Intake.h" -Intake::Intake(IntakeConfig config) : _config(config) {} +#include + +#include "frc/RobotController.h" +#include "units/base.h" + +Intake::Intake(IntakeConfig config) + : _config(config), + _pid(frc::PIDController(0.02, 0, 0, 0.05_s)), + _pidPosition(frc::PIDController(1.2, 0, 0, 0.05_s)) {} IntakeConfig Intake::GetConfig() { return _config; } +void Intake::OnStart() { + _pid.Reset(); + + _timer.Start(); +} + void Intake::OnUpdate(units::second_t dt) { switch (_state) { case IntakeState::kIdle: { if (_config.intakeSensor->Get() == false) { - setState(IntakeState::kHold); + SetState(IntakeState::kHold); } _stringStateName = "Idle"; + _pid.Reset(); _setVoltage = 0_V; + _recordNote = false; + hasValue = false; } break; case IntakeState::kRaw: { _stringStateName = "Raw"; + _pid.Reset(); _setVoltage = _rawVoltage; } break; case IntakeState::kEject: { - _stringStateName = "Eject"; - _setVoltage = 7_V; - if (_config.intakeSensor->Get() == true) { - setState(IntakeState::kIdle); + if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) { + SetState(IntakeState::kIdle); } + _stringStateName = "Eject"; + _setVoltage = 8_V; + _pid.Reset(); } break; case IntakeState::kHold: { + #ifdef DEBUG + std::cerr << "HEY FROM kHold" << std::endl; + #endif + units::volt_t pidCalculate = 0_V; + if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) { + SetState(IntakeState::kIdle); + } + // units::volt_t pidCalculate = + // units::volt_t{_pid.Calculate(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity().value())}; + if (_config.IntakeGearbox.encoder->GetEncoderPosition().value() < 0) { + pidCalculate = units::volt_t{ + -_pidPosition.Calculate(-_config.IntakeGearbox.encoder->GetEncoderPosition().value())}; + } else { + pidCalculate = units::volt_t{ + _pidPosition.Calculate(_config.IntakeGearbox.encoder->GetEncoderPosition().value())}; + } + + _setVoltage = pidCalculate; _stringStateName = "Hold"; - _setVoltage = 0_V; } break; case IntakeState::kIntake: { - _stringStateName = "Intake"; - _setVoltage = -7_V; if (_config.intakeSensor->Get() == false) { - setState(IntakeState::kHold); + if (_config.IntakeGearbox.encoder->GetEncoderPosition().value() < 0) { + _pidPosition.SetSetpoint((-_config.IntakeGearbox.encoder->GetEncoderPosition().value()) - 0.5); + } else { + _pidPosition.SetSetpoint(_config.IntakeGearbox.encoder->GetEncoderPosition().value() + 0.5); + } + + SetState(IntakeState::kHold); } + _stringStateName = "Intake"; + _setVoltage = -10_V; } break; case IntakeState::kPass: { - _stringStateName = "Pass"; - _setVoltage = -7_V; if (_config.intakeSensor->Get() == true) { - setState(IntakeState::kIdle); + SetState(IntakeState::kIdle); } + + if (!_recordNote) { + _noteShot++; + _recordNote = true; + } + + _stringStateName = "Pass"; + _setVoltage = -10_V; } break; - default: - std::cout << "Error: Intake in INVALID STATE." << std::endl; - break; } - _table->GetEntry("State: ").SetString(_stringStateName); - _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value()); - _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get()); - // _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get()); - // _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get()); - // _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get()); - // _table->GetEntry("Magazine Sensor: ").SetBoolean(_config.magSensor->Get()); - - std::cout << _setVoltage.value() << std::endl; + #ifdef DEBUG + _table->GetEntry("State").SetString(_stringStateName); + _table->GetEntry("Motor Voltage").SetDouble(_setVoltage.value()); + _table->GetEntry("Intake Sensor").SetBoolean(_config.intakeSensor->Get()); + _table->GetEntry("Pass Sensor").SetBoolean(_config.passSensor->Get()); + _table->GetEntry("Error").SetDouble(_pidPosition.GetPositionError()); + _table->GetEntry("SetPoint").SetDouble(_pidPosition.GetSetpoint()); + _table->GetEntry("Encoder").SetDouble(_config.IntakeGearbox.encoder->GetEncoderPosition().value()); + _table->GetEntry("Shot Count").SetDouble(_noteShot); + // _table->GetEntry("Encoder: ").SetDouble(_config.IntakeGearbox.encoder->GetEncoderPosition().value()); + #endif + if (_timer.Get() < 4_s) { + _setVoltage = units::math::min(0_V, _setVoltage); + } else { + _timer.Stop(); + _timer.Reset(); + // _timer.Restart(); + // _timer.Start(); + } - _config.IntakeMotor.motorController->SetVoltage(_setVoltage); + _config.IntakeGearbox.motorController->SetVoltage(_setVoltage); } -void Intake::setState(IntakeState state) { +void Intake::SetState(IntakeState state) { _state = state; } -void Intake::setRaw(units::volt_t voltage) { +void Intake::SetRaw(units::volt_t voltage) { _rawVoltage = voltage; } -IntakeState Intake::getState() { +IntakeState Intake::GetState() { return _state; } diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp index 8f704a1d..d69dd44b 100644 --- a/src/main/cpp/IntakeBehaviour.cpp +++ b/src/main/cpp/IntakeBehaviour.cpp @@ -6,64 +6,115 @@ #include -IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& codriver, LED* led) - : _intake(intake), _codriver(codriver), _led(led) { +#include "Intake.h" +#include "behaviour/Behaviour.h" +#include "networktables/NetworkTableInstance.h" + +IntakeManualControl::IntakeManualControl(Intake* intake, AlphaArm* arm, frc::XboxController& codriver) + : _intake(intake), _arm(arm), _codriver(codriver) { Controls(intake); } void IntakeManualControl::OnTick(units::second_t dt) { - if (_codriver.GetBButtonReleased()) { - if (_intake->getState() == IntakeState::kRaw) { - _intake->setState(IntakeState::kIdle); + if (_codriver.GetBackButtonReleased()) { + if (_rawControl) { + _rawControl = false; + _intake->SetState(IntakeState::kIdle); } else { - _intake->setState(IntakeState::kRaw); + _rawControl = true; + _intake->SetState(IntakeState::kRaw); } } - if (_intake->getState() == IntakeState::kRaw) { - if (_codriver.GetRightBumper()) { - _intake->setRaw(8_V); - } else if (_codriver.GetLeftBumper()) { - _intake->setRaw(-8_V); + if (_rawControl) { + _intake->SetState(IntakeState::kRaw); + if (_codriver.GetLeftTriggerAxis() > 0.1) { + _intake->SetRaw(_codriver.GetLeftTriggerAxis() * 10_V); + } else if (_codriver.GetRightTriggerAxis() > 0.1) { + _intake->SetRaw(_codriver.GetRightTriggerAxis() * -10_V); } else { - _intake->setRaw(0_V); + _intake->SetRaw(0_V); } - } else { - if (_codriver.GetRightBumperPressed()) { - if (_intake->getState() == IntakeState::kIntake) { - _intake->setState(IntakeState::kIdle); - _led->SetState(LEDState::kIdle); - } else { - _intake->setState(IntakeState::kIntake); - _led->SetState(LEDState::kIntaking); + if (_codriver.GetXButton()) { + if (_intake->GetState() == IntakeState::kIdle) { + _intake->SetState(IntakeState::kIntake); } - } - - if (_codriver.GetLeftBumper()) { - if (_intake->getState() == IntakeState::kEject) { - _intake->setState(IntakeState::kIdle); - _led->SetState(LEDState::kIdle); + } else if (_codriver.GetRightBumper() || _codriver.GetRightTriggerAxis() > 0.1) { + // if (_intake->GetState() == IntakeState::kHold) { + _intake->SetState(IntakeState::kPass); + // } else { + // _intake->SetState(IntakeState::kIdle); + // } + } else if (_codriver.GetBButton()) { + // if (_intake->GetState() == IntakeState::kHold) { + _intake->SetState(IntakeState::kEject); + // } else { + // _intake->SetState(IntakeState::kIdle); + // } + } else { + if (_intake->GetState() == IntakeState::kHold) { + _arm->SetState(AlphaArmState::kIntakedAngle); } else { - _intake->setState(IntakeState::kEject); - _led->SetState(LEDState::kIdle); + _intake->SetState(IntakeState::kIdle); } } + } +} - if (_codriver.GetAButtonPressed()) { - if (_intake->getState() == IntakeState::kPass) { - _intake->setState(IntakeState::kIdle); - _led->SetState(LEDState::kIdle); - } else { - _intake->setState(IntakeState::kPass); - _led->SetState(LEDState::kIntaking); - } - } +AutoIntake::AutoIntake(Intake* intake) : _intake(intake) { + Controls(intake); +} + +void AutoIntake::OnTick(units::second_t dt) { + _intake->SetState(IntakeState::kIntake); +} + +IntakeNote::IntakeNote(Intake* intake) : _intake(intake), behaviour::Behaviour("") { + Controls(intake); +} + +void IntakeNote::OnStart() { + _timer.Reset(); + _timer.Start(); +} + +void IntakeNote::OnTick(units::second_t dt) { + _intake->SetState(IntakeState::kIntake); + + // nt::NetworkTableInstance::GetDefault().GetTable("AutoIntake")->GetEntry("running").SetBoolean(true); + + // if (_intake->GetState() == IntakeState::kHold || _intake->GetState() == IntakeState::kIdle || + // _timer.Get() > 3_s) { _intake->SetState(IntakeState::kIdle); std::cerr << "EXITING" << std::endl; + SetDone(); + // } +} + +void IntakeNote::OnStop() { + // _intake->SetState(IntakeState::kIdle); + // nt::NetworkTableInstance::GetDefault().GetTable("AutoIntake")->GetEntry("running").SetBoolean(false); +} + +PassNote::PassNote(Intake* intake) : _intake(intake) { + Controls(intake); +} + +void PassNote::OnTick(units::second_t dt) { + _intake->SetState(IntakeState::kPass); + + if (_intake->GetState() == IntakeState::kIdle) { + SetDone(); } } -IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) { +EjectNote::EjectNote(Intake* intake) : _intake(intake) { Controls(intake); } -void IntakeAutoControl::OnTick(units::second_t dt) {} +void EjectNote::OnTick(units::second_t dt) { + _intake->SetState(IntakeState::kEject); + + if (_intake->GetState() == IntakeState::kIdle) { + SetDone(); + } +} diff --git a/src/main/cpp/LED.cpp b/src/main/cpp/LED.cpp index 337f6b44..a6de4524 100644 --- a/src/main/cpp/LED.cpp +++ b/src/main/cpp/LED.cpp @@ -13,7 +13,7 @@ void LED::OnUpdate(units::second_t dt) { } break; case LEDState::kAiming: { - _led.frc::PWM::SetSpeed(0.27); + _led.frc::PWM::SetSpeed(0.21); } break; case LEDState::kAmpReady: { _led.frc::PWM::SetSpeed(0.87); @@ -22,7 +22,7 @@ void LED::OnUpdate(units::second_t dt) { _led.frc::PWM::SetSpeed(0.57); } break; case LEDState::kIntaking: { - _led.frc::PWM::SetSpeed(0.07); + _led.frc::PWM::SetSpeed(0.93); } break; case LEDState::kShooterReady: { _led.frc::PWM::SetSpeed(0.77); diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 17b4148c..a146a88a 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -3,7 +3,7 @@ // of the MIT License at the root of this project #include "Robot.h" -#include "RobotMap.h" + #include #include #include @@ -13,108 +13,98 @@ #include #include // #include + +#include "Auto.h" +#include "RobotMap.h" + +// include units +#include +#include +#include #include #include #include #include - #include "behaviour/HasBehaviour.h" #include "networktables/NetworkTableInstance.h" +#include "behaviour/HasBehaviour.h" +#include +#include +#include +#include "behaviour/HasBehaviour.h" +#include "frc/geometry/Pose2d.h" static units::second_t lastPeriodic; void Robot::RobotInit() { - - // shooter = new Shooter(robotmap.shooterSystem.config); - // wom::BehaviourScheduler::GetInstance()->Register(shooter); - // shooter->SetDefaultBehaviour( - // [this]() { return wom::make(shooter, &robotmap.controllers.codriver); }); - - // sched = wom::BehaviourScheduler::GetInstance(); - // m_chooser.SetDefaultOption("Default Auto", "Default Auto"); - - // m_chooser.SetDefaultOption("Default Auto", "Default Auto"); - - frc::SmartDashboard::PutData("Auto Selector", &m_chooser); sched = wom::BehaviourScheduler::GetInstance(); - m_chooser.SetDefaultOption("Default Auto", "Default Auto"); - // m_path_chooser.SetDefaultOption("Path1", "paths/output/Path1.wpilib.json"); + frc::SmartDashboard::PutData("Auto Selector", &m_chooser); - // m_path_chooser.AddOption("Path1", "paths/output/Path1.wpilib.json"); - // m_path_chooser.AddOption("Path2", "paths/output/Path2.wpilib.json"); + m_chooser.SetDefaultOption(defaultAuto, defaultAuto); - // frc::SmartDashboard::PutData("Path Selector", &m_path_chooser); + for (auto& option : autoOptions) { + m_chooser.AddOption(option, option); + } - // frc::SmartDashboard::PutData("Field", &m_field); + _led = new LED(); - // simulation_timer = frc::Timer(); + armServo = new frc::Servo(3); - // robotmap.swerveBase.gyro->Reset(); - + shooter = new Shooter(robotmap.shooterSystem.config); + wom::BehaviourScheduler::GetInstance()->Register(shooter); + shooter->SetDefaultBehaviour( + [this]() { return wom::make(shooter, &robotmap.controllers.codriver, _led); }); - _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d()); + _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d(0_m, 0_m, 0_deg)); wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive); _swerveDrive->SetDefaultBehaviour( [this]() { return wom::make(_swerveDrive, &robotmap.controllers.driver); }); alphaArm = new AlphaArm(&robotmap.alphaArmSystem.config); - wom::BehaviourScheduler::GetInstance()->Register(alphaArm); - alphaArm->SetDefaultBehaviour( - [this]() { return wom::make(alphaArm, &robotmap.controllers.codriver); }); + intake = new Intake(robotmap.intakeSystem.config); - // robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0_rad); - // robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(0_rad); - // robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0_rad); - // robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0_rad); + wom::BehaviourScheduler::GetInstance()->Register(alphaArm); + alphaArm->SetDefaultBehaviour([this]() { + return wom::make(alphaArm, intake, &robotmap.controllers.codriver); + }); - // robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0.5948_rad); - // robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(2.6156_rad); - // robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(2.8931_rad); - // robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(-1.7417_rad); + climber = new Climber(robotmap.climberSystem.config); + wom::BehaviourScheduler::GetInstance()->Register(climber); + climber->SetDefaultBehaviour([this]() { + return wom::make(climber, alphaArm, &robotmap.controllers.codriver); + }); - // robotmap.swerveBase.moduleConfigs[0].driveMotor.motorController->SetInverted(true); - // robotmap.swerveBase.moduleConfigs[1].driveMotor.motorController->SetInverted(true); + wom::BehaviourScheduler::GetInstance()->Register(intake); + intake->SetDefaultBehaviour( + [this]() { return wom::make(intake, alphaArm, robotmap.controllers.codriver); }); - // robotmap.swerveBase.moduleConfigs[0].turnMotor.motorController->SetInverted(true); - // robotmap.swerveBase.moduleConfigs[1].turnMotor.motorController->SetInverted(true); robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0.45229_rad); robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(2.6846_rad); robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(3.01121_rad); robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(4.4524_rad); - // frontLeft = new ctre::phoenix6::hardware::TalonFX(7, "Drivebase"); // front left - // frontRight = new ctre::phoenix6::hardware::TalonFX(2, "Drivebase"); // front right - // backLeft = new ctre::phoenix6::hardware::TalonFX(6, "Drivebase"); // back left - // backRight = new ctre::phoenix6::hardware::TalonFX(4, "Drivebase"); // back right - // frontLeft = new ctre::phoenix6::hardware::TalonFX(9, "Drivebase"); // front left - // frontRight = new ctre::phoenix6::hardware::TalonFX(1, "Drivebase"); // front right - // backLeft = new ctre::phoenix6::hardware::TalonFX(5, "Drivebase"); // back left - // backRight = new ctre::phoenix6::hardware::TalonFX(3, "Drivebase"); lastPeriodic = wom::now(); - - // intake = new Intake(robotmap.intakeSystem.config); - // wom::BehaviourScheduler::GetInstance()->Register(intake); - // intake->SetDefaultBehaviour( - // [this]() { return wom::make(intake, robotmap.controllers.codriver); }); - - // _vision = new Vision("limelight", FMAP("fmap.fmap")); - - //robotmap->vision = new Vision("limelight", FMAP("fmap.fmap")); - } void Robot::RobotPeriodic() { + // double encoderDistance = robotmap.alphaArmSystem.armEncoder.GetDistance(); auto dt = wom::now() - lastPeriodic; lastPeriodic = wom::now(); loop.Poll(); wom::BehaviourScheduler::GetInstance()->Tick(); - //shooter->OnUpdate(dt); - //sched->Tick(); + + #ifdef DEBUG + nt::NetworkTableInstance::GetDefault() + .GetTable("drivetrainpose") + ->GetEntry("state") + .SetInteger(static_cast(_swerveDrive->GetState())); + #endif + // sched->Tick(); // robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder") // .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value()); @@ -126,31 +116,57 @@ void Robot::RobotPeriodic() { // .SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value()); // _swerveDrive->OnUpdate(dt); - //shooter->OnStart(); - //intake->OnUpdate(dt); + // shooter->OnStart(); + // intake->OnUpdate(dt); // _swerveDrive->OnUpdate(dt); - - robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 0 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value()); - robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 1 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value()); - robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 2 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value()); - robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 3 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value()); - + // robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 0 offset: + // ").SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value()); + // robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 1 offset: + // ").SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value()); + // robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 2 offset: + // ").SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value()); + // robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 3 offset: + // ").SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value()); + + _led->OnUpdate(dt); + shooter->OnUpdate(dt); alphaArm->OnUpdate(dt); _swerveDrive->OnUpdate(dt); + intake->OnUpdate(dt); + climber->OnUpdate(dt); } void Robot::AutonomousInit() { loop.Clear(); sched->InterruptAll(); + // _swerveDrive->MakeAtSetPoint(); + // _swerveDrive->SetPose(frc::Pose2d()); + + _swerveDrive->GetConfig().gyro->Reset(); + + m_autoSelected = m_chooser.GetSelected(); + + robotmap._builder = autos::InitCommands(_swerveDrive, shooter, intake, alphaArm); + + if (m_autoSelected == "kTaxi") { + sched->Schedule(autos::FourNoteTaxi(robotmap._builder)); + } + // sched->Schedule(robotmap._builder->GetAutoRoutine("OneNoteTaxi")); + + _swerveDrive->OnStart(); + + intake->OnStart(); } -void Robot::AutonomousPeriodic() {} +void Robot::AutonomousPeriodic() { + // robotmap._simSwerve->OnTick(_swerveDrive->GetSetpoint()); + // _swerveDrive->MakeAtSetPoint(); +} void Robot::TeleopInit() { loop.Clear(); - wom::BehaviourScheduler* sched = wom::BehaviourScheduler::GetInstance(); sched->InterruptAll(); @@ -159,47 +175,34 @@ void Robot::TeleopInit() { // backLeft->SetVoltage(4_V); // backRight->SetVoltage(4_V); - // FMAP("fmap.fmap"); - // _swerveDrive->OnStart(); + _swerveDrive->OnStart(); // sched->InterruptAll(); - //reimplement when vision is reimplemented + // reimplement when vision is reimplemented // _swerveDrive->SetPose(_vision->GetAngleToObject(VisionTargetObjects::kNote).first); + armServo->SetAngle(130); } -void Robot::TeleopPeriodic() {} +void Robot::TeleopPeriodic() { + if (robotmap.controllers.testController.GetPOV() == 0) { + armServo->SetAngle(130); + } else if (robotmap.controllers.codriver.GetPOV() == 270) { + climberTimer.Start(); + if (climberTimer.Get() > 1_s) { + armServo->SetAngle(0); + climberTimer.Stop(); + climberTimer.Reset(); + } + } + + robotmap.swerveTable.swerveDriveTable->GetEntry("SERVO POS").SetDouble(armServo->GetAngle()); +} void Robot::DisabledInit() {} void Robot::DisabledPeriodic() {} void Robot::TestInit() {} void Robot::TestPeriodic() {} - -void Robot::SimulationInit() { - /* std::string x = "["; - std::string y = "["; - // _vision->GetDistanceToTarget(16); - for (int i = 1; i < 17; i++) { - for (int j = 0; j < 17; j++) { - frc::Pose2d pose = _vision->AlignToTarget(i, units::meter_t{j * 0.1}, _swerveDrive); - x += std::to_string(pose.X().value()) + ","; - y += std::to_string(pose.Y().value()) + ","; - } - } - - x += "]"; - y += "]"; - - std::cout << x << std::endl; - std::cout << y << std::endl; */ - // std::cout << _vision->TurnToTarget(1, _swerveDrive).Rotation().Degrees().value() << std::endl; - //Reimplement when vision is reimplemented - // frc::Pose2d pose = _vision->TurnToTarget(2, _swerveDrive); - // nt::NetworkTableInstance::GetDefault().GetTable("vision")->PutNumber("rot", - // pose.Rotation().Degrees().value()); -} - -void Robot::SimulationPeriodic() {} diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp index 44326bda..8f05aa43 100644 --- a/src/main/cpp/Shooter.cpp +++ b/src/main/cpp/Shooter.cpp @@ -4,27 +4,30 @@ #include "Shooter.h" -Shooter::Shooter(ShooterConfig config) : _config(config), _pid(config.path + "/pid", config.pidConfig) {} +Shooter::Shooter(ShooterConfig config) : _config(config), _pid(frc::PIDController(0.02, 0.5, 0, 0.05_s)) {} void Shooter::OnStart() { _pid.Reset(); } void Shooter::OnUpdate(units::second_t dt) { - // _pid.SetTolerance(0.5, 4); - table->GetEntry("Error").SetDouble(_pid.GetError().value()); - // table->GetEntry("Acceleration Error").SetDouble(_pid.GetVelocityError()); - table->GetEntry("SetPoint").SetDouble(_pid.GetSetpoint().value()); - // table->GetEntry("Current - // Pos").SetDouble(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value()); - // table->GetEntry("EncoderValue").SetDouble(_config.ShooterGearbox.encoder->GetVelocityValue()); + #ifdef DEBUG + table->GetEntry("Error").SetDouble(_pid.GetPositionError()); + table->GetEntry("SetPoint").SetDouble(_pid.GetSetpoint()); + table->GetEntry("Encoder Output") + .SetDouble(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value()); table->GetEntry("Shooting").SetString(_statename); + // table->GetEntry("PID CONTROLLER").Set(_pid); +#endif + switch (_state) { case ShooterState::kIdle: { _statename = "Idle"; _pid.Reset(); holdVoltage = 0_V; + #ifdef DEBUG std::cout << "KIdle" << std::endl; + #endif _setVoltage = 0_V; // if (_shooterSensor.Get()) { // _state = ShooterState::kReverse; @@ -32,35 +35,45 @@ void Shooter::OnUpdate(units::second_t dt) { } break; case ShooterState::kSpinUp: { _statename = "SpinUp"; + #ifdef DEBUG std::cout << "KSpinUp" << std::endl; - _pid.SetSetpoint(_goal); + #endif + _pid.SetSetpoint(_goal.value()); units::volt_t pidCalculate = - units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)}; + units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value())}; + #ifdef DEBUG std::cout << "KShooting" << std::endl; + #endif - if (_pid.IsStable()) { - holdVoltage = pidCalculate; - std::cout << "STABLE" << std::endl; - _state = ShooterState::kShooting; - } + _setVoltage = pidCalculate; - if (holdVoltage.value() == 0) { - _setVoltage = pidCalculate; - } else { - _setVoltage = holdVoltage; - } + // if (_pid.GetPositionError() < 1 && _pid.GetVelocityError() < 1) { + // holdVoltage = pidCalculate; + // std::cout << "STABLE" << std::endl; + // _state = ShooterState::kShooting; + // } + + // if (holdVoltage.value() == 0) { + // _setVoltage = pidCalculate; + // } else { + // _setVoltage = holdVoltage; + // } } break; case ShooterState::kShooting: { _statename = "Shooting"; - _pid.SetSetpoint(_goal); + _pid.SetSetpoint(_goal.value()); units::volt_t pidCalculate = - units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)}; + units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value())}; + #ifdef DEBUG std::cout << "KShooting" << std::endl; + #endif - if (_pid.IsStable()) { + if (_pid.GetPositionError() < 1 && _pid.GetVelocityError() < 1) { holdVoltage = pidCalculate; + #ifdef DEBUG std::cout << "STABLE" << std::endl; + #endif } if (holdVoltage.value() == 0) { @@ -73,8 +86,10 @@ void Shooter::OnUpdate(units::second_t dt) { case ShooterState::kReverse: { _statename = "Reverse"; _pid.Reset(); - _setVoltage = -5_V; + _setVoltage = -8_V; + #ifdef DEBUG std::cout << "KReverse" << std::endl; + #endif // if (!_shooterSensor.Get()) { // SetState(ShooterState::kIdle); // } @@ -84,7 +99,9 @@ void Shooter::OnUpdate(units::second_t dt) { holdVoltage = 0_V; _pid.Reset(); _setVoltage = _rawVoltage; + #ifdef DEBUG std::cout << "KRaw" << std::endl; + #endif // if (_shooterSensor.Get()) { // SetState(ShooterState::kRaw); // } @@ -94,8 +111,6 @@ void Shooter::OnUpdate(units::second_t dt) { } break; } // table->GetEntry("Motor OutPut").SetDouble(_setVoltage.value()); - table->GetEntry("Encoder Output") - .SetDouble(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value()); _config.ShooterGearbox.motorController->SetVoltage(_setVoltage); } diff --git a/src/main/cpp/ShooterBehaviour.cpp b/src/main/cpp/ShooterBehaviour.cpp index 72a4cb54..91cd64d5 100644 --- a/src/main/cpp/ShooterBehaviour.cpp +++ b/src/main/cpp/ShooterBehaviour.cpp @@ -3,6 +3,12 @@ // of the MIT License at the root of this project #include "ShooterBehaviour.h" +#include "Intake.h" +#include "Shooter.h" +#include "behaviour/Behaviour.h" +#include "units/angle.h" +#include "units/angular_velocity.h" +#include "units/time.h" ShooterManualControl::ShooterManualControl(Shooter* shooter, frc::XboxController* tester, LED* led) : _shooter(shooter), _codriver(tester), _led(led) { @@ -12,7 +18,7 @@ ShooterManualControl::ShooterManualControl(Shooter* shooter, frc::XboxController void ShooterManualControl::OnTick(units::second_t dt) { table->GetEntry("RawControl").SetBoolean(_rawControl); - if (_codriver->GetBackButtonPressed()) { + if (_codriver->GetStartButtonPressed()) { if (_rawControl == true) { _rawControl = false; } else { @@ -22,33 +28,66 @@ void ShooterManualControl::OnTick(units::second_t dt) { if (_rawControl) { _shooter->SetState(ShooterState::kRaw); - if (_codriver->GetLeftTriggerAxis() > 0.1) { - _shooter->SetRaw(12_V * _codriver->GetLeftTriggerAxis()); - } else if (_codriver->GetRightTriggerAxis() > 0.1) { - _shooter->SetRaw(-12_V * _codriver->GetRightTriggerAxis()); + if (_codriver->GetRightBumper()) { + _shooter->SetRaw(8_V); + } else if (_codriver->GetLeftBumper()) { + _shooter->SetRaw(-8_V); } else { _shooter->SetRaw(0_V); } } else { - if (_codriver->GetXButton()) { - _shooter->SetPidGoal(150_rad_per_s); + if (_codriver->GetLeftTriggerAxis() > 0.1) { + _shooter->SetPidGoal(1500_rad_per_s); _shooter->SetState(ShooterState::kSpinUp); - _led->SetState(LEDState::kAiming); - } else if (_codriver->GetYButton()) { + } else if (_codriver->GetLeftBumper()) { _shooter->SetPidGoal(300_rad_per_s); _shooter->SetState(ShooterState::kSpinUp); _led->SetState(LEDState::kAiming); - } else if (_codriver->GetLeftTriggerAxis() > 0.1) { - _shooter->SetState(ShooterState::kRaw); - _led->SetState(LEDState::kIdle); - _shooter->SetRaw(12_V * _codriver->GetLeftTriggerAxis()); - } else if (_codriver->GetRightTriggerAxis() > 0.1) { - _shooter->SetState(ShooterState::kRaw); - _shooter->SetRaw(-12_V * _codriver->GetRightTriggerAxis()); - _led->SetState(LEDState::kIdle); + } else if (_codriver->GetBButton()) { + _shooter->SetState(ShooterState::kReverse); + + } else if (_codriver->GetAButton()) { + _shooter->SetPidGoal(1500_rad_per_s); + _shooter->SetState(ShooterState::kSpinUp); + _led->SetState(LEDState::kAiming); } else { _shooter->SetState(ShooterState::kIdle); _led->SetState(LEDState::kIdle); } } } + +AutoShooter::AutoShooter(Shooter* shooter, Intake* intake, units::radians_per_second_t goal) : behaviour::Behaviour(""), _shooter(shooter), _intake(intake), _goal(goal) { + Controls(shooter); +} + +void AutoShooter::OnTick(units::second_t dt) { + if (!_timer_started) { + _timer.Reset(); + _timer.Restart(); + _timer.Start(); + + _timer_started = true; + } + + _shooter->SetState(ShooterState::kSpinUp); + // _intake->SetState(IntakeState::kIntake); + + _shooter->SetPidGoal(_goal); + + if (_timer.Get() > 1_s) { + _intake->SetState(IntakeState::kPass); + + if (_timer.Get() > 1.9_s) { + _intake->SetState(IntakeState::kIdle); + _shooter->SetState(ShooterState::kIdle); + + SetDone(); + } + } +} + +void AutoShooter::OnStop() { + _shooter->SetState(ShooterState::kIdle); + _intake->SetState(IntakeState::kIdle); +} diff --git a/src/main/cpp/Vision.cpp b/src/main/cpp/Vision.cpp deleted file mode 100644 index 8684d8a4..00000000 --- a/src/main/cpp/Vision.cpp +++ /dev/null @@ -1,12 +0,0 @@ -// #include "Vision.h" - -// #include -// #include - -// #include "units/length.h" -// #include "units/math.h" -// #include "vision/Limelight.h" - -// //std::pair Vision::AngleToTarget(VisionTargetObjects objects){ - -// //} \ No newline at end of file diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp new file mode 100644 index 00000000..e7a11d1c --- /dev/null +++ b/src/main/cpp/vision/Vision.cpp @@ -0,0 +1,436 @@ +// Copyright (c) 2023-2024 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + +#include "vision/Vision.h" + +#include +#include + +#include "units/length.h" +#include "units/math.h" +#include "vision/Limelight.h" + +FMAP::FMAP(std::string path) : _path(path) { + #ifdef DEBUG + std::cout << "Parsing FMAP" << std::endl; + #endif + + deploy_dir = frc::filesystem::GetDeployDirectory(); + + std::ifstream file(deploy_dir + "/" + _path); + + // parse the json file into a wpi::json object + + wpi::json j; + + file >> j; + + #ifdef DEBUG + std::cout << j["fiducials"] << std::endl; + #endif + + // iterate through the json object and add each tag to the vector + + for (auto& fiducial : j["fiducials"]) { + #ifdef DEBUG + std::cout << "Loading AprilTag " << fiducial["id"] << std::endl; + #endif + AprilTag tag; + tag.id = fiducial["id"]; + tag.size = fiducial["size"]; + if (fiducial["unique"] == 1) { + tag.unique = true; + } else { + tag.unique = false; + } + + const auto& transform = fiducial["transform"]; + for (int i = 0; i < 4; ++i) { + for (int j = 0; j < 4; ++j) { + tag.transform[i][j] = transform[i * 4 + j]; + } + } + + tag.yaw = units::radian_t{tag.transform[0][0]}; + tag.pitch = units::radian_t{tag.transform[1][1]}; + tag.roll = units::radian_t{tag.transform[2][2]}; + + tag.pos = frc::Pose3d(units::meter_t{tag.transform[0][3]}, units::meter_t{tag.transform[1][3]}, + units::meter_t{tag.transform[2][3]}, frc::Rotation3d(0_deg, 0_deg, 0_deg)); + + _tags.push_back(tag); + } + + file.close(); + + + #ifdef DEBUG + std::cout << "Loaded " << _tags.size() << " tags" << std::endl; + #endif + + for (int i = 0; i < _tags.size(); i++) { + #ifdef DEBUG + std::cout << "Tag " << _tags[i].id << " is at: X: " << _tags[i].pos.X().value() + << " Y: " << _tags[i].pos.Y().value() << " Z: " << _tags[i].pos.Z().value() << std::endl; + #endif + } + + #ifdef DEBUG + std::cout << "Loaded FMAP" << std::endl; + #endif +} + +std::vector FMAP::GetTags() { + return _tags; +} + +Vision::Vision(std::string name, FMAP fmap) : _fmap(fmap) { + _limelight = new wom::Limelight(name); + + _limelight->SetPipeline(wom::LimelightPipeline::kPipeline0); +} + +frc::Pose3d Vision::GetPose() { + return _limelight->GetPose(); +} + +std::pair Vision::GetDistanceToTarget(VisionTarget target) { + SetMode(VisionModes::kAprilTag); + + std::vector tags = _fmap.GetTags(); + + for (int i = 0; i < tags.size(); i++) { + if (tags[i].id == static_cast(target)) { + AprilTag tag = tags[i]; + // frc::Pose3d pose = _limelight->GetPose(); + // frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg)); + + SetMode(VisionModes::kAprilTag); + + // Get distance to target + // Get current position from Limelight + frc::Pose2d current_pose = _limelight->GetPose().ToPose2d(); + // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg); + + units::meter_t a = tag.pos.X() - current_pose.X(); + units::meter_t b = tag.pos.Y() - current_pose.Y(); + + units::radian_t theta = units::math::atan2(b, a); + + // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl; + units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))}; + + // units::meter_t x = current_pose.X() + r * units::math::cos(theta); + // units::meter_t y = current_pose.Y() + r * units::math::sin(theta); + + return std::make_pair(r, theta); + } + } + + return std::make_pair(0_m, 0_deg); +} + +std::pair Vision::GetDistanceToTarget(int id) { + if (id < APRILTAGS_MIN || id > APRILTAGS_MAX) { + return std::make_pair(0_m, 0_deg); + } + + SetMode(VisionModes::kAprilTag); + + std::vector tags = _fmap.GetTags(); + + for (int i = 0; i < tags.size(); i++) { + if (tags[i].id == id) { + AprilTag tag = tags[i]; + // frc::Pose3d pose = _limelight->GetPose(); + // frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg)); + + SetMode(VisionModes::kAprilTag); + + // Get distance to target + // Get current position from Limelight + frc::Pose2d current_pose = _limelight->GetPose().ToPose2d(); + // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg); + + units::meter_t a = tag.pos.X() - current_pose.X(); + units::meter_t b = tag.pos.Y() - current_pose.Y(); + + units::radian_t theta = units::math::atan2(b, a); + + // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl; + units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))}; + + units::meter_t x = current_pose.X() + r * units::math::cos(theta); + units::meter_t y = current_pose.Y() + r * units::math::sin(theta); + + return std::make_pair(r, theta); + } + } + + return std::make_pair(0_m, 0_deg); +} + +std::vector Vision::GetTags() { + return _fmap.GetTags(); +} + +frc::Pose2d Vision::AlignToTarget(VisionTarget target, units::meter_t offset, wom::SwerveDrive* swerveDrive) { + SetMode(VisionModes::kAprilTag); + + // Get distance to target + AprilTag tag = GetTags()[static_cast(target) - 1]; + // Get current position from Limelight + frc::Pose2d current_pose = _limelight->GetPose().ToPose2d(); + // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg); + + units::meter_t a = tag.pos.X() - current_pose.X(); + units::meter_t b = tag.pos.Y() - current_pose.Y(); + + units::radian_t theta = units::math::atan2(b, a); + + // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl; + units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))}; + + units::meter_t x = current_pose.X() + r * units::math::cos(theta); + units::meter_t y = current_pose.Y() + r * units::math::sin(theta); + + // reduce both offsets by the offset parameter (in relative amount) + x += offset * units::math::cos(theta); + y += offset * units::math::sin(theta); + + frc::Pose2d pose = frc::Pose2d(x, y, theta); + + // Print the results + // std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << + // " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl; + + // Set the new pose to the swerve drive + // swerveDrive->SetPose(pose); + + return pose; +} + +frc::Pose2d Vision::AlignToTarget(int target, units::meter_t offset, wom::SwerveDrive* swerveDrive) { + SetMode(VisionModes::kAprilTag); + + // Get distance to target + std::pair distance = GetDistanceToTarget(target); + AprilTag tag = GetTags()[target - 1]; + // Get current position from Limelight + frc::Pose2d current_pose = _limelight->GetPose().ToPose2d(); + // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg); + + units::meter_t a = tag.pos.X() - current_pose.X(); + units::meter_t b = tag.pos.Y() - current_pose.Y(); + + units::radian_t theta = units::math::atan2(b, a); + + // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl; + units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))}; + + units::meter_t x = current_pose.X() + r * units::math::cos(theta); + units::meter_t y = current_pose.Y() + r * units::math::sin(theta); + + // reduce both offsets by the offset parameter (in relative amount) + x += offset * units::math::cos(theta); + y += offset * units::math::sin(theta); + + frc::Pose2d pose = frc::Pose2d(x, y, theta); + + // Print the results + // std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << + // " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl; + + // Set the new pose to the swerve drive + // swerveDrive->SetPose(pose); + + return pose; +} + +frc::Pose2d Vision::AlignToTarget(VisionTarget target, frc::Translation2d offset, + wom::SwerveDrive* swerveDrive) { + SetMode(VisionModes::kAprilTag); + + // Get distance to target + AprilTag tag = GetTags()[static_cast(target) - 1]; + // Get current position from Limelight + frc::Pose2d current_pose = _limelight->GetPose().ToPose2d(); + // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg); + + units::meter_t a = tag.pos.X() - current_pose.X(); + units::meter_t b = tag.pos.Y() - current_pose.Y(); + + units::radian_t theta = units::math::atan2(b, a); + + // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl; + units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))}; + + units::meter_t x = current_pose.X() + r * units::math::cos(theta); + units::meter_t y = current_pose.Y() + r * units::math::sin(theta); + + // reduce both offsets by the offset parameter (in relative amount) + x += offset.X(); + y += offset.Y(); + + frc::Pose2d pose = frc::Pose2d(x, y, theta); + + // Print the results + // std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << + // " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl; + + // Set the new pose to the swerve drive + // swerveDrive->SetPose(pose); + + return pose; +} + +frc::Pose2d Vision::AlignToTarget(int target, frc::Translation2d offset, wom::SwerveDrive* swerveDrive) { + SetMode(VisionModes::kAprilTag); + + // Get distance to target + std::pair distance = GetDistanceToTarget(target); + AprilTag tag = GetTags()[target - 1]; + // Get current position from Limelight + frc::Pose2d current_pose = _limelight->GetPose().ToPose2d(); + // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg); + + units::meter_t a = tag.pos.X() - current_pose.X(); + units::meter_t b = tag.pos.Y() - current_pose.Y(); + + units::radian_t theta = units::math::atan2(b, a); + + // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl; + units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))}; + + units::meter_t x = current_pose.X() + r * units::math::cos(theta); + units::meter_t y = current_pose.Y() + r * units::math::sin(theta); + + // reduce both offsets by the offset parameter (in relative amount) + x += offset.X(); + y += offset.Y(); + + frc::Pose2d pose = frc::Pose2d(x, y, theta); + + // Print the results + // std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << + // " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl; + + // Set the new pose to the swerve drive + // swerveDrive->SetPose(pose); + + return pose; +} + +frc::Pose2d Vision::TurnToTarget(int target, wom::SwerveDrive* swerveDrive) { + AprilTag tag = GetTags()[target]; + + units::degree_t angle = GetDistanceToTarget(target).second; + + frc::Pose2d current_pose = _limelight->GetPose().ToPose2d(); + // frc::Pose2d current_pose = frc::Pose2d(); + + frc::Pose2d pose = frc::Pose2d(current_pose.X(), current_pose.Y(), angle); + + // std::cout << pose.Rotation().Degrees().value() << std::endl; + + swerveDrive->TurnToAngle(angle); + + return pose; +} + +frc::Pose2d Vision::TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive) { + AprilTag tag = GetTags()[static_cast(target)]; + + units::degree_t angle = GetDistanceToTarget(target).second; + + frc::Pose2d current_pose = _limelight->GetPose().ToPose2d(); + + // frc::Pose2d current_pose = swerveDrive->GetPose(); + + frc::Pose2d pose = frc::Pose2d(current_pose.X(), current_pose.Y(), angle); + + #ifdef DEBUG + std::cout << pose.Rotation().Degrees().value() << std::endl; + #endif + + swerveDrive->TurnToAngle(angle); + + return pose; +} + +std::pair Vision::GetAngleToObject(VisionTargetObjects object) { + SetMode(VisionModes::kRing); + + if (TargetIsVisible(object)) { + frc::Pose2d pose = _limelight->GetPose().ToPose2d(); + + units::degree_t offset = units::degree_t{ + _limelight->GetOffset() + .first}; // degrees are how much the robot has to turn for the target to be in the center + + pose.RotateBy(offset); + + return std::make_pair(pose, offset); + } else { + return std::make_pair(frc::Pose2d(), 0_deg); + } +} + +units::degree_t Vision::LockOn(VisionTargetObjects object) { + SetMode(VisionModes::kRing); + + units::degree_t angle; + + if (TargetIsVisible(object)) { + angle = GetAngleToObject(object).second; + } + + return angle; +} + +bool Vision::IsAtPose(frc::Pose3d pose, units::second_t dt) { + return _limelight->IsAtSetPoseVision(pose, dt); +} + +bool Vision::IsAtPose(frc::Pose2d pose, units::second_t dt) { + return _limelight->IsAtSetPoseVision(pose, dt); +} + +void Vision::SetMode(VisionModes mode) { + if (static_cast(mode) == _limelight->GetTargetingData(wom::LimelightTargetingData::kGetpipe)) { + return; + } + + switch (mode) { + case VisionModes::kAprilTag: { + _limelight->SetPipeline(wom::LimelightPipeline::kPipeline0); + break; + } + case VisionModes::kRing: { + _limelight->SetPipeline(wom::LimelightPipeline::kPipeline1); + break; + } + } +} + +bool Vision::TargetIsVisible(VisionTargetObjects target) { + switch (target) { + case VisionTargetObjects::kNote: { + SetMode(VisionModes::kRing); + break; + } + } + + return _limelight->HasTarget(); +} + +int Vision::CurrentAprilTag() { + SetMode(VisionModes::kAprilTag); + + return _limelight->GetTargetingData(wom::LimelightTargetingData::kTid); +} + +wom::Limelight* Vision::GetLimelight() { + return _limelight; +} diff --git a/src/main/cpp/vision/VisionBehaviours.cpp b/src/main/cpp/vision/VisionBehaviours.cpp new file mode 100644 index 00000000..ab2d8724 --- /dev/null +++ b/src/main/cpp/vision/VisionBehaviours.cpp @@ -0,0 +1,67 @@ +// Copyright (c) 2023-2024 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + +#include "vision/VisionBehaviours.h" + +#include "frc/geometry/Pose2d.h" +#include "units/angle.h" +#include "vision/Vision.h" + +AlignToAprilTag::AlignToAprilTag(Vision* vision, VisionTarget target, wom::SwerveDrive* swerveDrive, + units::meter_t offset) + : _vision(vision), _swerveDrive(swerveDrive), _target(target), _offset(offset) {} + +void AlignToAprilTag::OnTick(units::second_t dt) { + frc::Pose2d pose = _vision->AlignToTarget(_target, _offset, _swerveDrive); + + if (_vision->IsAtPose(pose, dt)) { + SetDone(); + } +} + +TurnToAprilTag::TurnToAprilTag(Vision* vision, VisionTarget target, wom::SwerveDrive* swerveDrive) + : _vision(vision), _swerveDrive(swerveDrive), _target(target) {} + +void TurnToAprilTag::OnTick(units::second_t dt) { + frc::Pose2d pose = _vision->TurnToTarget(_target, _swerveDrive); + + if (_vision->IsAtPose(pose, dt)) { + SetDone(); + } +} + +LockOnToTarget::LockOnToTarget(Vision* vision, VisionTargetObjects target, wom::SwerveDrive* swerveDrive) + : _vision(vision), + _target(target), + _camera(nullptr), + _swerveDrive(swerveDrive), + _type(VisionType::kLimelight) {} + +LockOnToTarget::LockOnToTarget(wom::PhotonVision* vision, Vision* limelight, wom::SwerveDrive* swerveDrive) + : _camera(vision), _swerveDrive(swerveDrive), _vision(limelight), _type(VisionType::kGenericCamera) {} + +void LockOnToTarget::OnTick(units::second_t dt) { + if (!_vision->TargetIsVisible(VisionTargetObjects::kNote)) { + SetDone(); + } + + units::degree_t angle; + + switch (_type) { + case VisionType::kLimelight: { + angle = _vision->LockOn(_target); + } + case VisionType::kGenericCamera: { + angle = units::degree_t{_camera->GetTargetPitch(_camera->GetTarget())}; + } + } + + frc::Pose2d pose = frc::Pose2d(_swerveDrive->GetPose().X(), _swerveDrive->GetPose().Y(), angle); + + _swerveDrive->SetPose(pose); + + if (_vision->IsAtPose(pose, dt)) { + SetDone(); + } +} diff --git a/src/main/deploy/fmap.fmap b/src/main/deploy/fmap.fmap new file mode 100644 index 00000000..9cb43e6d --- /dev/null +++ b/src/main/deploy/fmap.fmap @@ -0,0 +1,388 @@ +{ + "fiducials": [ + { + "family": "apriltag3_36h11_classic", + "id": 1, + "size": 165.1, + "transform": [ + -0.5, + -0.866025, + 0, + 6.808597, + 0.866025, + -0.5, + 0, + -3.859403, + 0, + 0, + 1, + 1.355852, + 0, + 0, + 0, + 1 + ], + "unique": 1 + }, + { + "family": "apriltag3_36h11_classic", + "id": 2, + "size": 165.1, + "transform": [ + -0.5, + -0.866025, + 0, + 7.914259, + 0.866025, + -0.5, + 0, + -3.221609, + 0, + 0, + 1, + 1.355852, + 0, + 0, + 0, + 1 + ], + "unique": 1 + }, + { + "family": "apriltag3_36h11_classic", + "id": 3, + "size": 165.1, + "transform": [ + -1.0, + 0, + 0, + 8.308467, + 0, + -1.0, + 0, + 0.877443, + 0, + 0, + 1, + 1.451102, + 0, + 0, + 0, + 1 + ], + "unique": 1 + }, + { + "family": "apriltag3_36h11_classic", + "id": 4, + "size": 165.1, + "transform": [ + -1.0, + 0, + 0, + 8.308467, + 0, + -1.0, + 0, + 1.442593, + 0, + 0, + 1, + 1.451102, + 0, + 0, + 0, + 1 + ], + "unique": 1 + }, + { + "family": "apriltag3_36h11_classic", + "id": 5, + "size": 165.1, + "transform": [ + 0, + 1.0, + 0, + 6.429883, + -1.0, + 0, + 0, + 4.098925, + 0, + 0, + 1, + 1.355852, + 0, + 0, + 0, + 1 + ], + "unique": 1 + }, + { + "family": "apriltag3_36h11_classic", + "id": 6, + "size": 165.1, + "transform": [ + 0, + 1.0, + 0, + -6.429375, + -1.0, + 0, + 0, + 4.098925, + 0, + 0, + 1, + 1.355852, + 0, + 0, + 0, + 1 + ], + "unique": 1 + }, + { + "family": "apriltag3_36h11_classic", + "id": 7, + "size": 165.1, + "transform": [ + 1.0, + -0.0, + 0, + -8.308975, + 0.0, + 1.0, + 0, + 1.442593, + 0, + 0, + 1, + 1.451102, + 0, + 0, + 0, + 1 + ], + "unique": 1 + }, + { + "family": "apriltag3_36h11_classic", + "id": 8, + "size": 165.1, + "transform": [ + 1.0, + -0.0, + 0, + -8.308975, + 0.0, + 1.0, + 0, + 0.877443, + 0, + 0, + 1, + 1.451102, + 0, + 0, + 0, + 1 + ], + "unique": 1 + }, + { + "family": "apriltag3_36h11_classic", + "id": 9, + "size": 165.1, + "transform": [ + 0.5, + -0.866025, + 0, + -7.914767, + 0.866025, + 0.5, + 0, + -3.221609, + 0, + 0, + 1, + 1.355852, + 0, + 0, + 0, + 1 + ], + "unique": 1 + }, + { + "family": "apriltag3_36h11_classic", + "id": 10, + "size": 165.1, + "transform": [ + 0.5, + -0.866025, + 0, + -6.809359, + 0.866025, + 0.5, + 0, + -3.859403, + 0, + 0, + 1, + 1.355852, + 0, + 0, + 0, + 1 + ], + "unique": 1 + }, + { + "family": "apriltag3_36h11_classic", + "id": 11, + "size": 165.1, + "transform": [ + 0.5, + 0.8660254, + 0, + 3.633851, + -0.8660254, + 0.5, + 0, + -0.392049, + 0, + 0, + 1, + 1.3208, + 0, + 0, + 0, + 1 + ], + "unique": 1 + }, + { + "family": "apriltag3_36h11_classic", + "id": 12, + "size": 165.1, + "transform": [ + 0.5, + -0.866025, + 0, + 3.633851, + 0.866025, + 0.5, + 0, + 0.393065, + 0, + 0, + 1, + 1.3208, + 0, + 0, + 0, + 1 + ], + "unique": 1 + }, + { + "family": "apriltag3_36h11_classic", + "id": 13, + "size": 165.1, + "transform": [ + -1.0, + 0, + 0, + 2.949321, + 0, + -1.0, + 0, + -0.000127, + 0, + 0, + 1, + 1.3208, + 0, + 0, + 0, + 1 + ], + "unique": 1 + }, + { + "family": "apriltag3_36h11_classic", + "id": 14, + "size": 165.1, + "transform": [ + 1.0, + -0.0, + 0, + -2.950083, + 0.0, + 1.0, + 0, + -0.000127, + 0, + 0, + 1, + 1.3208, + 0, + 0, + 0, + 1 + ], + "unique": 1 + }, + { + "family": "apriltag3_36h11_classic", + "id": 15, + "size": 165.1, + "transform": [ + -0.5, + -0.866025, + 0, + -3.629533, + 0.866025, + -0.5, + 0, + 0.393065, + 0, + 0, + 1, + 1.3208, + 0, + 0, + 0, + 1 + ], + "unique": 1 + }, + { + "family": "apriltag3_36h11_classic", + "id": 16, + "size": 165.1, + "transform": [ + -0.5, + 0.866025, + 0, + -3.629533, + -0.866025, + -0.5, + 0, + -0.392049, + 0, + 0, + 1, + 1.3208, + 0, + 0, + 0, + 1 + ], + "unique": 1 + } + ] +} diff --git a/src/main/deploy/pathplanner/autos/FourNote.auto b/src/main/deploy/pathplanner/autos/FourNote.auto new file mode 100644 index 00000000..b452b4d8 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/FourNote.auto @@ -0,0 +1,97 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.9789443299448194, + "y": 5.556063249807658 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "FirstNote" + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "path", + "data": { + "pathName": "CenterBackToStart" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "CloseLeft" + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "path", + "data": { + "pathName": "CloseLeftReturn" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "CloseRight" + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "path", + "data": { + "pathName": "CloseRightReturn" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/FourNoteFar.auto b/src/main/deploy/pathplanner/autos/FourNoteFar.auto new file mode 100644 index 00000000..7a3134a1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/FourNoteFar.auto @@ -0,0 +1,91 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7850516674765234, + "y": 6.3027226484706595 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "LeftCloseLeft" + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "FarLeft" + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "path", + "data": { + "pathName": "FarLeftReturn" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "FarLeftCenter" + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "path", + "data": { + "pathName": "FarLeftCenterReturn" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/FourNoteTaxi.auto b/src/main/deploy/pathplanner/autos/FourNoteTaxi.auto new file mode 100644 index 00000000..b2a19a5b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/FourNoteTaxi.auto @@ -0,0 +1,103 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.9789443299448194, + "y": 5.556063249807658 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "FirstNoteGood" + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "path", + "data": { + "pathName": "CenterBackToStart" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "CloseLeft" + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "path", + "data": { + "pathName": "CloseLeftReturn" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "CloseRight" + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "path", + "data": { + "pathName": "CloseRightReturn" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "FirstNoteGood" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/FourNoteTaxiBad.auto b/src/main/deploy/pathplanner/autos/FourNoteTaxiBad.auto new file mode 100644 index 00000000..21c3ab0f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/FourNoteTaxiBad.auto @@ -0,0 +1,136 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "FirstNote" + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "BackwardsWorse" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "named", + "data": { + "name": "TurnToSecondNote" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "GoToSecondNote" + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "BackFromSecondNote" + } + }, + { + "type": "named", + "data": { + "name": "TurnToSpeakerFromSecond" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "named", + "data": { + "name": "TurnToThirdNote" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "GoToThirdNote" + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "BackFromThirdNote" + } + }, + { + "type": "named", + "data": { + "name": "TurnToSpeakerFromThird" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/LeftOneNote.auto b/src/main/deploy/pathplanner/autos/LeftOneNote.auto new file mode 100644 index 00000000..3757a645 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/LeftOneNote.auto @@ -0,0 +1,43 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7850516674765234, + "y": 6.3027226484706595 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "LeftCloseLeft" + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/OneNote.auto b/src/main/deploy/pathplanner/autos/OneNote.auto new file mode 100644 index 00000000..50f060f1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/OneNote.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.37, + "y": 5.56 + }, + "rotation": 0.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/OneNoteTaxi.auto b/src/main/deploy/pathplanner/autos/OneNoteTaxi.auto new file mode 100644 index 00000000..61000c9e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/OneNoteTaxi.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Backwards" + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Taxi.auto b/src/main/deploy/pathplanner/autos/Taxi.auto new file mode 100644 index 00000000..0976f300 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Taxi.auto @@ -0,0 +1,50 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "FirstNote" + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "BackwardsWorse" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/ThreeNote.auto b/src/main/deploy/pathplanner/autos/ThreeNote.auto new file mode 100644 index 00000000..6159ba17 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/ThreeNote.auto @@ -0,0 +1,73 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.9789443299448194, + "y": 5.556063249807658 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "FirstNote" + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "path", + "data": { + "pathName": "CenterBackToStart" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "CloseLeft" + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "path", + "data": { + "pathName": "CloseLeftReturn" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/ThreeNoteFar.auto b/src/main/deploy/pathplanner/autos/ThreeNoteFar.auto new file mode 100644 index 00000000..6c448793 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/ThreeNoteFar.auto @@ -0,0 +1,67 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7850516674765234, + "y": 6.3027226484706595 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "LeftCloseLeft" + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "FarLeft" + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "path", + "data": { + "pathName": "FarLeftReturn" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/ThreeNoteTaxi.auto b/src/main/deploy/pathplanner/autos/ThreeNoteTaxi.auto new file mode 100644 index 00000000..72836db4 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/ThreeNoteTaxi.auto @@ -0,0 +1,73 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.9789443299448194, + "y": 5.556063249807658 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "FirstNoteGood" + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "path", + "data": { + "pathName": "CenterBackToStart" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "CloseLeft" + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "path", + "data": { + "pathName": "CloseLeftReturn" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/ThreeNoteTaxiBad.auto b/src/main/deploy/pathplanner/autos/ThreeNoteTaxiBad.auto new file mode 100644 index 00000000..35422857 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/ThreeNoteTaxiBad.auto @@ -0,0 +1,81 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "FirstNote" + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "BackwardsWorse" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "GoToSecondNote" + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "BackFromSecondNote" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TwoNote.auto b/src/main/deploy/pathplanner/autos/TwoNote.auto new file mode 100644 index 00000000..a33df241 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/TwoNote.auto @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.9789443299448194, + "y": 5.556063249807658 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "FirstNote" + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "path", + "data": { + "pathName": "CenterBackToStart" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TwoNoteTaxi.auto b/src/main/deploy/pathplanner/autos/TwoNoteTaxi.auto new file mode 100644 index 00000000..bdc155b0 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/TwoNoteTaxi.auto @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.9789443299448194, + "y": 5.556063249807658 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "FirstNote" + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "path", + "data": { + "pathName": "CenterBackToStart" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "FirstNote" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 00000000..d49bb0f7 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} diff --git a/src/main/deploy/pathplanner/paths/BackFromSecondNote.path b/src/main/deploy/pathplanner/paths/BackFromSecondNote.path new file mode 100644 index 00000000..cb0d9c08 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BackFromSecondNote.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.2, + "y": 1.8 + }, + "prevControl": null, + "nextControl": { + "x": 0.24924928537446073, + "y": 0.4117108365457816 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.0, + "y": 0.0 + }, + "prevControl": { + "x": 0.40057438174790605, + "y": 0.5504093176517296 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 100.0, + "maxAngularAcceleration": 150.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 56.42020002146988, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BackFromThirdNote.path b/src/main/deploy/pathplanner/paths/BackFromThirdNote.path new file mode 100644 index 00000000..c25fa3f8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BackFromThirdNote.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3, + "y": -1.8 + }, + "prevControl": null, + "nextControl": { + "x": 0.3492492853744608, + "y": -3.1882891634542183 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.0, + "y": 0.0 + }, + "prevControl": { + "x": 0.40057438174790605, + "y": 0.5504093176517296 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 100.0, + "maxAngularAcceleration": 150.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 56.42020002146988, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Backwards.path b/src/main/deploy/pathplanner/paths/Backwards.path new file mode 100644 index 00000000..f27afc50 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Backwards.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 0.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.993173655558508, + "y": 0.020888794958252705 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.0, + "y": 0.0 + }, + "prevControl": { + "x": -1.0, + "y": 0.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 100.0, + "maxAngularAcceleration": 150.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} diff --git a/src/main/deploy/pathplanner/paths/BackwardsWorse.path b/src/main/deploy/pathplanner/paths/BackwardsWorse.path new file mode 100644 index 00000000..825b0b13 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BackwardsWorse.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.7, + "y": 0.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.693173655558508, + "y": 0.020888794958252705 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.0, + "y": 0.0 + }, + "prevControl": { + "x": -1.0, + "y": 0.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 100.0, + "maxAngularAcceleration": 150.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CenterBackToStart.path b/src/main/deploy/pathplanner/paths/CenterBackToStart.path new file mode 100644 index 00000000..e3908cd4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CenterBackToStart.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.9066934025451485, + "y": 5.574358195791997 + }, + "prevControl": null, + "nextControl": { + "x": 3.2727390112774013, + "y": 5.574358195791997 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.36086297850199, + "y": 5.574358195791997 + }, + "prevControl": { + "x": 0.622987338824146, + "y": 5.574358195791997 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 100.0, + "maxAngularAcceleration": 150.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 3.094058058917208, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} diff --git a/src/main/deploy/pathplanner/paths/CloseLeft.path b/src/main/deploy/pathplanner/paths/CloseLeft.path new file mode 100644 index 00000000..c9e73a17 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CloseLeft.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.9778908835444483, + "y": 5.52001197299496 + }, + "prevControl": null, + "nextControl": { + "x": 1.536685674141276, + "y": 5.599313077022893 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.830961626597167, + "y": 7.010989778320136 + }, + "prevControl": { + "x": 1.8309616265971669, + "y": 7.010989778320136 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 100.0, + "maxAngularAcceleration": 150.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} diff --git a/src/main/deploy/pathplanner/paths/CloseLeftReturn.path b/src/main/deploy/pathplanner/paths/CloseLeftReturn.path new file mode 100644 index 00000000..1c720683 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CloseLeftReturn.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.830961626597167, + "y": 7.010989778320136 + }, + "prevControl": null, + "nextControl": { + "x": 3.3897564171939947, + "y": 7.090290882348069 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.8831977571110939, + "y": 5.494436635385951 + }, + "prevControl": { + "x": -0.11680224288890606, + "y": 5.494436635385951 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 100.0, + "maxAngularAcceleration": 150.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} diff --git a/src/main/deploy/pathplanner/paths/CloseRight.path b/src/main/deploy/pathplanner/paths/CloseRight.path new file mode 100644 index 00000000..2f23ffba --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CloseRight.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.9464630659333784, + "y": 5.519836398594897 + }, + "prevControl": null, + "nextControl": { + "x": 1.9464630659333793, + "y": 5.519836398594897 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.7818593193798713, + "y": 4.096396212493511 + }, + "prevControl": { + "x": 1.7818593193798713, + "y": 4.096396212493511 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 100.0, + "maxAngularAcceleration": 150.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} diff --git a/src/main/deploy/pathplanner/paths/CloseRightReturn.path b/src/main/deploy/pathplanner/paths/CloseRightReturn.path new file mode 100644 index 00000000..cf2db296 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CloseRightReturn.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.7818593193798713, + "y": 4.096396212493511 + }, + "prevControl": null, + "nextControl": { + "x": 3.7818593193798726, + "y": 4.096396212493511 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.973560048342923, + "y": 5.571630846613142 + }, + "prevControl": { + "x": -0.02643995165707702, + "y": 5.571630846613142 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 100.0, + "maxAngularAcceleration": 150.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} diff --git a/src/main/deploy/pathplanner/paths/FarLeft.path b/src/main/deploy/pathplanner/paths/FarLeft.path new file mode 100644 index 00000000..d0f89c01 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/FarLeft.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.830961626597167, + "y": 7.010989778320136 + }, + "prevControl": null, + "nextControl": { + "x": 3.3897564171939933, + "y": 7.090290882348069 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.215594377293828, + "y": 7.475676690483815 + }, + "prevControl": { + "x": 8.724373097296924, + "y": 7.4628597592793 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 100.0, + "maxAngularAcceleration": 150.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} diff --git a/src/main/deploy/pathplanner/paths/FarLeftCenter.path b/src/main/deploy/pathplanner/paths/FarLeftCenter.path new file mode 100644 index 00000000..1c7868b6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/FarLeftCenter.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.830961626597167, + "y": 7.010989778320136 + }, + "prevControl": null, + "nextControl": { + "x": 3.3897564171939933, + "y": 7.090290882348069 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.277396566115597, + "y": 5.782437176287395 + }, + "prevControl": { + "x": 8.786175286118693, + "y": 5.76962024508288 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 100.0, + "maxAngularAcceleration": 150.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} diff --git a/src/main/deploy/pathplanner/paths/FarLeftCenterReturn.path b/src/main/deploy/pathplanner/paths/FarLeftCenterReturn.path new file mode 100644 index 00000000..b9faf6a5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/FarLeftCenterReturn.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.277396566115597, + "y": 5.782437176287395 + }, + "prevControl": null, + "nextControl": { + "x": 8.836191356712423, + "y": 5.861738280315328 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.800002007386261, + "y": 6.960014677502181 + }, + "prevControl": { + "x": 3.308780727389358, + "y": 6.9471977462976655 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 100.0, + "maxAngularAcceleration": 150.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} diff --git a/src/main/deploy/pathplanner/paths/FarLeftReturn.path b/src/main/deploy/pathplanner/paths/FarLeftReturn.path new file mode 100644 index 00000000..5b0b2b5c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/FarLeftReturn.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.215594377293828, + "y": 7.475676690483815 + }, + "prevControl": null, + "nextControl": { + "x": 8.774389167890654, + "y": 7.554977794511748 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.838452800999805, + "y": 7.002562207117168 + }, + "prevControl": { + "x": 3.3472315210029024, + "y": 6.989745275912653 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 100.0, + "maxAngularAcceleration": 150.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} diff --git a/src/main/deploy/pathplanner/paths/FirstNote.path b/src/main/deploy/pathplanner/paths/FirstNote.path new file mode 100644 index 00000000..6317d90a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/FirstNote.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.0, + "y": -0.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.0, + "y": 0.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.8, + "y": -0.0 + }, + "prevControl": { + "x": 0.8, + "y": 0.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 100.0, + "maxAngularAcceleration": 150.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FirstNoteGood.path b/src/main/deploy/pathplanner/paths/FirstNoteGood.path new file mode 100644 index 00000000..27f75bf1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/FirstNoteGood.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.33, + "y": 5.5040931765173085 + }, + "prevControl": null, + "nextControl": { + "x": 2.329999999999999, + "y": 5.5040931765173085 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.918404239090931, + "y": 5.5040931765173085 + }, + "prevControl": { + "x": 1.9184042390909308, + "y": 5.5040931765173085 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 100.0, + "maxAngularAcceleration": 150.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/GoToSecondNote.path b/src/main/deploy/pathplanner/paths/GoToSecondNote.path new file mode 100644 index 00000000..d4fd37d3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/GoToSecondNote.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.0, + "y": 0.0 + }, + "prevControl": null, + "nextControl": { + "x": 0.9744053724911999, + "y": 1.346746202764874 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2, + "y": 1.8 + }, + "prevControl": { + "x": 0.4483335066527455, + "y": 0.7044817501903187 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 100.0, + "maxAngularAcceleration": 150.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 52.11278203254082, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 50.75873366922985, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/GoToThirdNote.path b/src/main/deploy/pathplanner/paths/GoToThirdNote.path new file mode 100644 index 00000000..cade06f7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/GoToThirdNote.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.0, + "y": 0.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.2733841279990303, + "y": -1.0684961521509886 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3, + "y": -1.8 + }, + "prevControl": { + "x": 0.2822377369416278, + "y": -0.945996060441566 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 100.0, + "maxAngularAcceleration": 150.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 52.11278203254082, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 50.75873366922985, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeftCloseLeft.path b/src/main/deploy/pathplanner/paths/LeftCloseLeft.path new file mode 100644 index 00000000..c5219045 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LeftCloseLeft.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7850516674765234, + "y": 6.3027226484706595 + }, + "prevControl": null, + "nextControl": { + "x": 1.3438464580733505, + "y": 6.3820237524985926 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.830961626597167, + "y": 7.010989778320136 + }, + "prevControl": { + "x": 1.8309616265971669, + "y": 7.010989778320136 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 100.0, + "maxAngularAcceleration": 150.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} diff --git a/src/main/include/AlphaArm.h b/src/main/include/AlphaArm.h index 8429fa9d..d7f9a89a 100644 --- a/src/main/include/AlphaArm.h +++ b/src/main/include/AlphaArm.h @@ -4,14 +4,13 @@ #pragma once #include - +#include +#include +#include #include "Wombat.h" -//#include "vision/Vision.h" #include "utils/PID.h" -#include -#include -#include +// #include "vision/Vision.h" struct AlphaArmConfig { wom::Gearbox alphaArmGearbox; @@ -20,7 +19,6 @@ struct AlphaArmConfig { std::string path; // Vision *vision; - }; enum class AlphaArmState { @@ -31,33 +29,37 @@ enum class AlphaArmState { kHoldAngle, kVisionAngle, kStowed, - kRaw + kClimbAngle, + kClimbed, + kRaw, + kIntakedAngle }; class AlphaArm : public behaviour::HasBehaviour { public: - AlphaArm(AlphaArmConfig *config/*, frc::Rotation2d initialAngle, wom::vision::Limelight* vision*/); + AlphaArm(AlphaArmConfig* config /*, frc::Rotation2d initialAngle, wom::vision::Limelight* vision*/); void OnUpdate(units::second_t dt); void SetArmRaw(units::volt_t voltage); void SetState(AlphaArmState state); - void SetControllerRaw(units::volt_t voltage); + void SetControllerRaw(units::volt_t voltage); void SetGoal(double goal); void OnStart(); - AlphaArmConfig GetConfig(); //{ return _config; } + AlphaArmConfig GetConfig(); //{ return _config; } frc::PIDController GetPID(); private: // units::radian_t CalcTargetAngle(); - AlphaArmConfig *_config; - wom::vision::Limelight* _vision; + AlphaArmConfig* _config; + // wom::vision::Limelight* _vision; AlphaArmState _state = AlphaArmState::kIdle; - //wom::utils::PIDController _alphaArmPID; - //frc::DutyCycleEncoder armEncoder{4}; + // wom::utils::PIDController _alphaArmPID; + // frc::DutyCycleEncoder armEncoder{4}; frc::PIDController _pidArm; frc::PIDController _pidArmStates; frc::PIDController _pidIntakeState; + frc::PIDController _pidClimberStates; std::shared_ptr _table = nt::NetworkTableInstance::GetDefault().GetTable("AlphaArm"); units::volt_t _setAlphaArmVoltage = 0_V; @@ -65,5 +67,4 @@ class AlphaArm : public behaviour::HasBehaviour { units::volt_t _rawArmVoltage = 0_V; units::volt_t _testRawVoltage = 3_V; double _goal = 0; - }; diff --git a/src/main/include/AlphaArmBehaviour.h b/src/main/include/AlphaArmBehaviour.h index f29714c3..3016cdb0 100644 --- a/src/main/include/AlphaArmBehaviour.h +++ b/src/main/include/AlphaArmBehaviour.h @@ -2,26 +2,27 @@ // Open Source Software, you can modify it according to the terms // of the MIT License at the root of this project -#pragma once - #pragma once #include #include "AlphaArm.h" +#include "Intake.h" #include "Wombat.h" +#include "vision/Vision.h" class AlphaArmManualControl : public behaviour::Behaviour { public: - explicit AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver); + explicit AlphaArmManualControl(AlphaArm* alphaArm, Intake* intake, frc::XboxController* codriver); void OnTick(units::second_t dt); private: AlphaArm* _alphaArm; + Intake* _intake; frc::XboxController* _codriver; units::volt_t _setAlphaArmVoltage = 0_V; - bool _rawControl; + bool _rawControl = false; bool _gotValue = false; - std::shared_ptr _table = nt::NetworkTableInstance::GetDefault().GetTable("AlphaArmSubsystem"); - + std::shared_ptr _table = + nt::NetworkTableInstance::GetDefault().GetTable("AlphaArmSubsystem"); + bool climbing = false; }; - diff --git a/src/main/include/ArmAngle.h b/src/main/include/ArmAngle.h deleted file mode 100644 index 051565db..00000000 --- a/src/main/include/ArmAngle.h +++ /dev/null @@ -1,84 +0,0 @@ -// // Copyright (c) 2023-2024 CurtinFRC -// // Open Source Software, you can modify it according to the terms -// // of the MIT License at the root of this project - -// #pragma once - -// #include -// #include -// #include -// #include -// #include - -// #include -// #include -// #include - -// #include "Wombat.h" -// #include "units/angle.h" -// #include "AlphaArm.h" - -// #define APRILTAGS_MAX 16 -// #define APRILTAGS_MIN 0 - -// enum class VisionTarget { -// /* -// Left is toward the blue side of the diagram here: -// https://firstfrc.blob.core.windows.net/frc2024/FieldAssets/2024LayoutMarkingDiagram.pdf - -// The numbers are the numbers on the field diagram (and the numbers on the tags). -// */ -// kBlueAmp = 6, -// kBlueSpeakerCenter = 7, -// kBlueSpeakerOffset = 8, -// kBlueChain1 = 15, -// kBlueChain2 = 16, -// kBlueChain3 = 14, -// kBlueSourceLeft = 1, -// kBlueSourceRight = 2, - -// kRedAmp = 5, -// kRedSpeakerCenter = 4, -// kRedSpeakerOffset = 3, - -// kRedChain1 = 12, -// kRedChain2 = 11, -// kRedChain3 = 13, -// kRedSourceLeft = 9, -// kRedSourceRight = 10 -// }; - -// enum class VisionModes { kAprilTag = 0, kRing = 1 }; - -// enum class VisionTargetObjects { kNote }; - -// struct AprilTag { -// int id; -// double size; -// std::array, 4> transform; -// bool unique; - -// frc::Pose3d pos; -// units::radian_t yaw; -// units::radian_t pitch; -// units::radian_t roll; -// }; - -// class FMAP { -// public: -// explicit FMAP(std::string path); - -// std::vector GetTags(); - -// private: -// std::vector _tags; -// std::string _path; -// std::string deploy_dir; -// }; - -// class ArmVision{ -// public: -// ArmVision(std::string name, FMAP fmap); - -// frc::Twist2d AngleToAprilTag(VisionTarget target, double angleOffset, ); -// }; diff --git a/src/main/include/Auto.h b/src/main/include/Auto.h new file mode 100644 index 00000000..93fe9b69 --- /dev/null +++ b/src/main/include/Auto.h @@ -0,0 +1,44 @@ +// Copyright (c) 2023-2024 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + +#pragma once + +#include + +#include "AlphaArmBehaviour.h" +#include "IntakeBehaviour.h" +#include "ShooterBehaviour.h" +#include "Wombat.h" +#include "utils/Pathplanner.h" + +namespace autos { + +// wom::Commands* _commands = nullptr; +// wom::SwerveAutoBuilder* builder = nullptr; + +wom::utils::SwerveAutoBuilder* InitCommands(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, + Intake* _intake, AlphaArm* _alphaArm); + +std::shared_ptr AutoTest(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, + Intake* _intake, AlphaArm* _alphaArm); + +std::shared_ptr Taxi(wom::utils::SwerveAutoBuilder* builder); +std::shared_ptr OneNoteTaxi(wom::utils::SwerveAutoBuilder* builder); +std::shared_ptr FourNoteTaxi(wom::utils::SwerveAutoBuilder* builder); +std::shared_ptr ThreeNoteTaxi(wom::SwerveAutoBuilder* builder); + +std::shared_ptr QuadrupleClose(wom::drivetrain::SwerveDrive* _swerveDrive, + Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm); + +std::shared_ptr QuadrupleFar(wom::drivetrain::SwerveDrive* _swerveDrive, + Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm); + +std::shared_ptr QuadrupleCloseDoubleFar(wom::drivetrain::SwerveDrive* _swerveDrive, + Shooter* _shooter, Intake* _intake, + AlphaArm* _alphaArm); + +std::shared_ptr QuadrupleCloseSingleFar(wom::drivetrain::SwerveDrive* _swerveDrive, + Shooter* _shooter, Intake* _intake, + AlphaArm* _alphaArm); +} // namespace autos diff --git a/src/main/include/Climber.h b/src/main/include/Climber.h new file mode 100644 index 00000000..4928f6e9 --- /dev/null +++ b/src/main/include/Climber.h @@ -0,0 +1,38 @@ +// Copyright (c) 2023-2024 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + +#pragma once + +#include + +#include +#include + +#include "Wombat.h" + +struct ClimberConfig { + wom::Gearbox climberGearbox; +}; + +enum class ClimberState { kIdle, kArmUp, kArmDown, kMatch, kRaw, kRatchet }; + +class Climber : public behaviour::HasBehaviour { + public: + explicit Climber(ClimberConfig config); + + void OnUpdate(units::second_t dt); + void SetState(ClimberState state); + ClimberState GetState(); + void SetRaw(units::volt_t voltage); + + private: + ClimberConfig _config; + ClimberState _state = ClimberState::kMatch; + units::volt_t _rawVoltage = 0_V; + std::string _stringStateName = "error"; + units::volt_t _setVoltage = 0_V; + std::shared_ptr _table = nt::NetworkTableInstance::GetDefault().GetTable("Climber"); + + frc::PIDController _pid; +}; diff --git a/src/main/include/ClimberBehaviour.h b/src/main/include/ClimberBehaviour.h new file mode 100644 index 00000000..2f2487f7 --- /dev/null +++ b/src/main/include/ClimberBehaviour.h @@ -0,0 +1,24 @@ +// Copyright (c) 2023-2024 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + +#pragma once + +#include + +#include "AlphaArm.h" +#include "Climber.h" +#include "Wombat.h" + +class ClimberManualControl : public behaviour::Behaviour { + public: + ClimberManualControl(Climber* climber, AlphaArm* arm, frc::XboxController* codriver); + void OnTick(units::second_t dt) override; + + private: + Climber* _climber; + AlphaArm* _arm; + frc::XboxController* _codriver; + + bool _rawControl = false; +}; diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h index 8d2a07d3..4765ccb9 100644 --- a/src/main/include/Intake.h +++ b/src/main/include/Intake.h @@ -5,17 +5,22 @@ #pragma once #include +#include +#include #include #include #include "Wombat.h" +#include "frc/Timer.h" struct IntakeConfig { - wom::Gearbox IntakeMotor; + std::string path; + wom::Gearbox IntakeGearbox; frc::DigitalInput* intakeSensor; - frc::DigitalInput* magSensor; - frc::DigitalInput* shooterSensor; + frc::DigitalInput* passSensor; + + wom::PIDConfig pidConfig; }; enum class IntakeState { kIdle, kRaw, kHold, kEject, kIntake, kPass }; @@ -26,18 +31,29 @@ class Intake : public behaviour::HasBehaviour { void OnUpdate(units::second_t dt); - void setState(IntakeState state); - void setRaw(units::volt_t voltage); - IntakeState getState(); + void SetState(IntakeState state); + void SetRaw(units::volt_t voltage); + void OnStart(); + IntakeState GetState(); IntakeConfig GetConfig(); private: IntakeConfig _config; IntakeState _state = IntakeState::kIdle; + int _noteShot = 0; + + bool _recordNote = false; + bool hasValue = false; + units::volt_t _rawVoltage = 0_V; std::string _stringStateName = "error"; units::volt_t _setVoltage = 0_V; + frc::PIDController _pid; + frc::PIDController _pidPosition; + std::shared_ptr _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake"); + + frc::Timer _timer; }; diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h index a94af750..fbbc4747 100644 --- a/src/main/include/IntakeBehaviour.h +++ b/src/main/include/IntakeBehaviour.h @@ -6,33 +6,58 @@ #include +#include "AlphaArm.h" #include "Intake.h" -#include "LED.h" #include "Wombat.h" class IntakeManualControl : public behaviour::Behaviour { public: - explicit IntakeManualControl(Intake* intake, frc::XboxController& codriver, LED* led); + explicit IntakeManualControl(Intake* intake, AlphaArm* arm, frc::XboxController& codriver); void OnTick(units::second_t dt) override; private: Intake* _intake; + AlphaArm* _arm; frc::XboxController& _codriver; - LED* _led; + bool _rawControl = false; +}; - units::volt_t _rawVoltage; - units::volt_t _setVoltage; +class AutoIntake : public behaviour::Behaviour { + public: + explicit AutoIntake(Intake* intake); + void OnTick(units::second_t dt) override; + + private: + Intake* _intake; }; -class IntakeAutoControl : public behaviour::Behaviour { +class IntakeNote : public behaviour::Behaviour { public: - explicit IntakeAutoControl(Intake* intake); + explicit IntakeNote(Intake* intake); + void OnTick(units::second_t dt) override; + void OnStop() override; + void OnStart() override; + + private: + Intake* _intake; + frc::Timer _timer; +}; +class PassNote : public behaviour::Behaviour { + public: + explicit PassNote(Intake* intake); void OnTick(units::second_t dt) override; private: Intake* _intake; +}; - LED* _led; +class EjectNote : public behaviour::Behaviour { + public: + explicit EjectNote(Intake* intake); + void OnTick(units::second_t dt) override; + + private: + Intake* _intake; }; diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index ec00a96c..e6c0dec6 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -3,7 +3,9 @@ // of the MIT License at the root of this project #pragma once -// #include + +#include +#include #include #include #include @@ -12,13 +14,26 @@ #include #include #include +#include +#include +#include #include +#include #include "AlphaArm.h" #include "AlphaArmBehaviour.h" +#include "Auto.h" +#include "Climber.h" +#include "ClimberBehaviour.h" +#include "Intake.h" +#include "IntakeBehaviour.h" +#include "LED.h" #include "RobotMap.h" +#include "Shooter.h" +#include "ShooterBehaviour.h" #include "Wombat.h" +#include "vision/Vision.h" class Robot : public frc::TimedRobot { public: @@ -32,18 +47,36 @@ class Robot : public frc::TimedRobot { void TeleopPeriodic() override; void DisabledInit() override; void DisabledPeriodic() override; - void SimulationInit() override; - void SimulationPeriodic() override; + // void SimulationInit() override; + // void SimulationPeriodic() override; private: RobotMap robotmap; wom::BehaviourScheduler* sched; frc::EventLoop loop; - //Shooter* shooter; + Shooter* shooter; + + Intake* intake; + Climber* climber; + + frc::Timer climberTimer; + Vision* vision; - // Intake* intake; frc::SendableChooser 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 autoOptions = { + kTaxi, kAutoTest, kQuadrupleClose, kQuadrupleFar, kQuadrupleCloseDoubleFar, kQuadrupleCloseSingleFar, + }; + // frc::Field2d m_field; // frc::Timer simulation_timer; @@ -54,21 +87,25 @@ class Robot : public frc::TimedRobot { // rev::CANSparkMax testMotorUp{1, rev::CANSparkMax::MotorType::kBrushless}; // rev::CANSparkMax testMotorDown{6, rev::CANSparkMax::MotorType::kBrushless}; - // frc::XboxController testdriver = frc::XboxController(1); + // frc::XboxController testdriver = frc::XboxController(1); AlphaArm* alphaArm; - + LED* _led; // ctre::phoenix6::hardware::TalonFX *frontLeft; // ctre::phoenix6::hardware::TalonFX *frontRight; // ctre::phoenix6::hardware::TalonFX *backLeft; // ctre::phoenix6::hardware::TalonFX *backRight; - //wom::SwerveDrive* _swerveDrive; + // wom::SwerveDrive* _swerveDrive; + frc::Servo* armServo; - //ctre::phoenix6::hardware::TalonFX *frontLeft; - // ctre::phoenix6::hardware::TalonFX *frontRight; - // ctre::phoenix6::hardware::TalonFX *backLeft; - // ctre::phoenix6::hardware::TalonFX *backRight; + // ctre::phoenix6::hardware::TalonFX *frontLeft; + // ctre::phoenix6::hardware::TalonFX *frontRight; + // ctre::phoenix6::hardware::TalonFX *backLeft; + // ctre::phoenix6::hardware::TalonFX *backRight; + // ctre::phoenix6::hardware::TalonFX *frontLeft; + // ctre::phoenix6::hardware::TalonFX *frontRight; + // ctre::phoenix6::hardware::TalonFX *backLeft; + // ctre::phoenix6::hardware::TalonFX *backRight; }; - diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h index 842da856..91ea79ad 100644 --- a/src/main/include/RobotMap.h +++ b/src/main/include/RobotMap.h @@ -2,34 +2,39 @@ // Open Source Software, you can modify it according to the terms // of the MIT License at the root of this project +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + #pragma once #include +#include #include #include #include #include -#include #include +#include +#include #include #include #include #include -#include "AlphaArm.h" #include #include #include +// #include #include "AlphaArm.h" #include "AlphaArmBehaviour.h" +#include "Climber.h" #include "Intake.h" #include "Shooter.h" #include "Wombat.h" -#include "Wombat.h" -#include "AlphaArm.h" -#include "AlphaArmBehaviour.h" +#include "utils/Encoder.h" +#include "utils/PID.h" struct RobotMap { struct Controllers { @@ -37,9 +42,10 @@ struct RobotMap { frc::XboxController codriver = frc::XboxController(1); frc::XboxController testController = frc::XboxController(2); }; + Controllers controllers; - - struct AlphaArmSystem { + + struct AlphaArmSystem { rev::CANSparkMax alphaArmMotorUp{21, rev::CANSparkMax::MotorType::kBrushless}; rev::CANSparkMax alphaArmMotorDown{26, rev::CANSparkMax::MotorType::kBrushless}; @@ -47,26 +53,44 @@ struct RobotMap { wom::CANSparkMaxEncoder* alphaArmNeoEncoderUp = new wom::CANSparkMaxEncoder(&alphaArmMotorUp, 0.1_m); wom::CANSparkMaxEncoder* alphaArmNeoEncoderDown = new wom::CANSparkMaxEncoder(&alphaArmMotorDown, 0.1_m); - wom::Gearbox alphaArmGearbox{&alphaArmMotorUp, alphaArmNeoEncoderUp, frc::DCMotor::NEO(1)}; wom::Gearbox alphaArmGearbox2{&alphaArmMotorDown, alphaArmNeoEncoderDown, frc::DCMotor::NEO(1)}; - AlphaArmConfig config{alphaArmGearbox, alphaArmGearbox2, alphaArmEncoder, "/alphaArm" - //, &vision + AlphaArmConfig config{ + alphaArmGearbox, alphaArmGearbox2, alphaArmEncoder, "/alphaArm" + //, &vision }; }; AlphaArmSystem alphaArmSystem; + struct IntakeSystem { + rev::CANSparkMax intakeMotor{35, rev::CANSparkMax::MotorType::kBrushless}; + wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m}; + frc::DigitalInput intakeSensor{7}; + frc::DigitalInput passSensor{5}; + // frc::DigitalInput magSensor{0}; + // frc::DigitalInput shooterSensor{0}; + + wom::Gearbox IntakeGearbox{&intakeMotor, &intakeEncoder, frc::DCMotor::NEO(1)}; + + IntakeConfig config{ + "path", + IntakeGearbox, + &intakeSensor, + &passSensor, + wom::PIDConfig("/intake/PID/config", 9_V / (180_deg / 1_s), + 0_V / 25_deg, 0_V / (90_deg / 1_s / 1_s)), + /*, &magSensor, &shooterSensor*/}; + }; + IntakeSystem intakeSystem; - struct SwerveBase { ctre::phoenix6::hardware::CANcoder frontLeftCancoder{16, "Drivebase"}; ctre::phoenix6::hardware::CANcoder frontRightCancoder{18, "Drivebase"}; ctre::phoenix6::hardware::CANcoder backLeftCancoder{17, "Drivebase"}; ctre::phoenix6::hardware::CANcoder backRightCancoder{19, "Drivebase"}; - ctre::phoenix6::hardware::Pigeon2* gyro = - new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase"); + ctre::phoenix6::hardware::Pigeon2* gyro = new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase"); wpi::array turnMotors{ new ctre::phoenix6::hardware::TalonFX(6, "Drivebase"), // front left new ctre::phoenix6::hardware::TalonFX(7, "Drivebase"), // front right @@ -79,48 +103,38 @@ struct RobotMap { new ctre::phoenix6::hardware::TalonFX(1, "Drivebase")}; // back right wpi::array moduleConfigs{ - wom::SwerveModuleConfig{ //CORRECT + wom::SwerveModuleConfig{ + // CORRECT // front left module frc::Translation2d(-10_in, 9_in), - wom::Gearbox{ - driveMotors[0], - new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75), - frc::DCMotor::Falcon500(1).WithReduction(6.75)}, - wom::Gearbox{turnMotors[0], - new wom::CanEncoder(16, 0.0445_m, 4096, 12.8), + wom::Gearbox{driveMotors[0], new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75), + frc::DCMotor::Falcon500(1).WithReduction(6.75)}, + wom::Gearbox{turnMotors[0], new wom::CanEncoder(16, 0.0445_m, 4096, 12.8), frc::DCMotor::Falcon500(1).WithReduction(12.8)}, &frontLeftCancoder, 4_in / 2}, - wom::SwerveModuleConfig{ //CORRECT + wom::SwerveModuleConfig{ + // CORRECT // front right module frc::Translation2d(10_in, 9_in), - wom::Gearbox{ - driveMotors[1], - new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75), - frc::DCMotor::Falcon500(1).WithReduction(6.75)}, - wom::Gearbox{turnMotors[1], - new wom::CanEncoder(18, 0.0445_m, 4096, 12.8), + wom::Gearbox{driveMotors[1], new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75), + frc::DCMotor::Falcon500(1).WithReduction(6.75)}, + wom::Gearbox{turnMotors[1], new wom::CanEncoder(18, 0.0445_m, 4096, 12.8), frc::DCMotor::Falcon500(1).WithReduction(12.8)}, &frontRightCancoder, 4_in / 2}, wom::SwerveModuleConfig{ // back left module frc::Translation2d(-10_in, 9_in), - wom::Gearbox{ - driveMotors[2], - new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75), - frc::DCMotor::Falcon500(1).WithReduction(6.75)}, - wom::Gearbox{turnMotors[2], - new wom::CanEncoder(17, 0.0445_m, 4096, 12.8), + wom::Gearbox{driveMotors[2], new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75), + frc::DCMotor::Falcon500(1).WithReduction(6.75)}, + wom::Gearbox{turnMotors[2], new wom::CanEncoder(17, 0.0445_m, 4096, 12.8), frc::DCMotor::Falcon500(1).WithReduction(12.8)}, &backRightCancoder, 4_in / 2}, wom::SwerveModuleConfig{ // back right module frc::Translation2d(-10_in, -9_in), - wom::Gearbox{ - driveMotors[3], - new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75), - frc::DCMotor::Falcon500(1).WithReduction(6.75)}, - wom::Gearbox{turnMotors[3], - new wom::CanEncoder(19, 0.0445_m, 4096, 12.8), + wom::Gearbox{driveMotors[3], new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75), + frc::DCMotor::Falcon500(1).WithReduction(6.75)}, + wom::Gearbox{turnMotors[3], new wom::CanEncoder(19, 0.0445_m, 4096, 12.8), frc::DCMotor::Falcon500(1).WithReduction(12.8)}, &backLeftCancoder, 4_in / 2}, }; @@ -140,19 +154,16 @@ struct RobotMap { wom::SwerveDriveConfig::pose_angle_conf_t::ki_t{0}, 0_deg / 1_deg};*/ wom::SwerveDriveConfig::pose_position_conf_t posePositionPID{ - "/drivetrain/pid/pose/position/config", - 0_mps / 1_m, - wom::SwerveDriveConfig::pose_position_conf_t::ki_t{0.15}, - 0_m / 1_m, - 0_cm}; + "/drivetrain/pid/pose/position/config", 0_mps / 1_m, + wom::SwerveDriveConfig::pose_position_conf_t::ki_t{0.15}, 0_m / 1_m, 0_cm}; // the config for the whole swerve drive wom::SwerveDriveConfig config{"/drivetrain", - //anglePID, + // anglePID, velocityPID, moduleConfigs, // each module gyro, - //poseAnglePID, + // poseAnglePID, posePositionPID, 60_kg, // robot mass (estimate rn) {0.1, 0.1, 0.1}, @@ -172,7 +183,41 @@ struct RobotMap { SwerveBase swerveBase; struct SwerveTable { - std::shared_ptr swerveDriveTable = nt::NetworkTableInstance::GetDefault().GetTable("swerve"); - }; SwerveTable swerveTable; -}; + std::shared_ptr swerveDriveTable = + nt::NetworkTableInstance::GetDefault().GetTable("swerve"); + }; + SwerveTable swerveTable; + + struct Shooter { + rev::CANSparkMax shooterMotor{31, rev::CANSparkMax::MotorType::kBrushless}; // Port 11 + // frc::DigitalInput shooterSensor{2}; + // wom::VoltageController shooterMotorGroup = wom::VoltagedController::Group(shooterMotor); + wom::CANSparkMaxEncoder* shooterEncoder = new wom::CANSparkMaxEncoder(&shooterMotor, 0.01_m); + wom::Gearbox shooterGearbox{&shooterMotor, shooterEncoder, frc::DCMotor::NEO(1)}; + + ShooterConfig config{ + "shooterGearbox", + shooterGearbox, + }; + }; + Shooter shooterSystem; + + struct ClimberSystem { + rev::CANSparkMax climberMotor{32, rev::CANSparkMax::MotorType::kBrushless}; + // wom::utils::CANSparkMaxEncoder climberEncoder{&climberMotor, 0.1_m}; + wom::CANSparkMaxEncoder* climberEncoder = new wom::CANSparkMaxEncoder(&climberMotor, 0.1_m); + + // // frc::DigitalInput climberSensor{99}; + // // wom::MotorVoltageController climberMotorController = + // wom::MotorVoltageController::Group(climberMotor); + wom::Gearbox climberGearbox{&climberMotor, climberEncoder, frc::DCMotor::NEO(1)}; + ClimberConfig config { + climberGearbox, + }; + }; ClimberSystem climberSystem; + + wom::SwerveAutoBuilder* _builder; + wom::SimSwerve* _simSwerve; +}; +// AlphaArmSystem alphaArmSystem; diff --git a/src/main/include/Shooter.h b/src/main/include/Shooter.h index d2c22cb5..b2a2ce9c 100644 --- a/src/main/include/Shooter.h +++ b/src/main/include/Shooter.h @@ -17,7 +17,6 @@ struct ShooterConfig { wom::Gearbox ShooterGearbox; // wom::PIDConfig pidConfig; // frc::DigitalInput* shooterSensor; - wom::PIDConfig pidConfig; }; enum class ShooterState { kIdle, kShooting, kSpinUp, kReverse, kRaw }; @@ -37,13 +36,14 @@ class Shooter : public behaviour::HasBehaviour { private: ShooterConfig _config; std::shared_ptr table = nt::NetworkTableInstance::GetDefault().GetTable("Shooter"); - ShooterState _state = ShooterState::kRaw; + ShooterState _state = ShooterState::kIdle; units::volt_t _rawVoltage; units::radians_per_second_t _goal; units::volt_t _setVoltage = 0_V; - wom::utils::PIDController _pid; // frc::DigitalInput _shooterSensor{0}; + frc::PIDController _pid; + units::volt_t holdVoltage = 0_V; std::string _statename = "default"; }; diff --git a/src/main/include/ShooterBehaviour.h b/src/main/include/ShooterBehaviour.h index d5ace50c..dfce34b8 100644 --- a/src/main/include/ShooterBehaviour.h +++ b/src/main/include/ShooterBehaviour.h @@ -5,12 +5,15 @@ #pragma once #include +#include #include +#include "Intake.h" #include "LED.h" #include "Shooter.h" #include "Wombat.h" +#include "frc/Timer.h" class ShooterManualControl : public behaviour::Behaviour { public: @@ -28,3 +31,19 @@ class ShooterManualControl : public behaviour::Behaviour { std::shared_ptr table = nt::NetworkTableInstance::GetDefault().GetTable("Shooter Behaviour"); }; + +class AutoShooter : public behaviour::Behaviour { + public: + AutoShooter(Shooter* shooter, Intake* intake, units::radians_per_second_t goal); + + void OnTick(units::second_t dt) override; + void OnStop() override; + + private: + Shooter* _shooter; + Intake* _intake; + units::radians_per_second_t _goal; + + frc::Timer _timer; + bool _timer_started = false; +}; diff --git a/src/main/include/Vision.h b/src/main/include/Vision.h deleted file mode 100644 index ea953fb9..00000000 --- a/src/main/include/Vision.h +++ /dev/null @@ -1,111 +0,0 @@ -// // Copyright (c) 2023-2024 CurtinFRC -// // Open Source Software, you can modify it according to the terms -// // of the MIT License at the root of this project - -// #pragma once - -// #include -// #include -// #include -// #include -// #include - -// #include -// #include -// #include - -// #include "Wombat.h" -// #include "units/angle.h" -// #include "AlphaArm.h" - -// #define APRILTAGS_MAX 16 -// #define APRILTAGS_MIN 0 - -// enum class VisionTarget { -// /* -// Left is toward the blue side of the diagram here: -// https://firstfrc.blob.core.windows.net/frc2024/FieldAssets/2024LayoutMarkingDiagram.pdf - -// The numbers are the numbers on the field diagram (and the numbers on the tags). -// */ -// kBlueAmp = 6, -// kBlueSpeakerCenter = 7, -// kBlueSpeakerOffset = 8, -// kBlueChain1 = 15, -// kBlueChain2 = 16, -// kBlueChain3 = 14, -// kBlueSourceLeft = 1, -// kBlueSourceRight = 2, - -// kRedAmp = 5, -// kRedSpeakerCenter = 4, -// kRedSpeakerOffset = 3, - -// kRedChain1 = 12, -// kRedChain2 = 11, -// kRedChain3 = 13, -// kRedSourceLeft = 9, -// kRedSourceRight = 10 -// }; - -// enum class VisionModes { kAprilTag = 0, kRing = 1 }; - -// enum class VisionTargetObjects { kNote }; - -// struct AprilTag { -// int id; -// double size; -// std::array, 4> transform; -// bool unique; - -// frc::Pose3d pos; -// units::radian_t yaw; -// units::radian_t pitch; -// units::radian_t roll; -// }; - -// class FMAP { -// public: -// explicit FMAP(std::string path); - -// std::vector GetTags(); - -// private: -// std::vector _tags; -// std::string _path; -// std::string deploy_dir; -// }; - -// class Vision { -// public: -// Vision(std::string name, FMAP fmap); - -// std::pair GetDistanceToTarget(VisionTarget target); -// std::pair GetDistanceToTarget(int id); -// //void GetAngleToTarget(double angle); - -// void SetMode(VisionModes mode); - -// frc::Pose3d GetPose(); - -// frc::Rotation2d AngleToTarget(VisionTarget target, double angle, AlphaArm* alphaArm); -// std::pair AngleToTarget(VisionTarget target); -// double LockOn(VisionTarget target); - -// //std::pair GetAngleToTarget(Vision) - -// std::pair GetAngleToObject(VisionTargetObjects object); -// units::degree_t LockOn(VisionTargetObjects target); - -// std::vector GetTags(); - -// bool IsAtPose(frc::Pose3d pose, units::second_t dt); - -// bool TargetIsVisible(VisionTargetObjects target); - -// int CurrentAprilTag(); - -// private: -// wom::Limelight* _limelight; -// FMAP _fmap; -// }; \ No newline at end of file diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h new file mode 100644 index 00000000..731bca21 --- /dev/null +++ b/src/main/include/vision/Vision.h @@ -0,0 +1,115 @@ +// Copyright (c) 2023-2024 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "Wombat.h" +#include "units/angle.h" + +#define APRILTAGS_MAX 16 +#define APRILTAGS_MIN 0 + +enum class VisionTarget { + /* + Left is toward the blue side of the diagram here: + https://firstfrc.blob.core.windows.net/frc2024/FieldAssets/2024LayoutMarkingDiagram.pdf + + The numbers are the numbers on the field diagram (and the numbers on the tags). + */ + kBlueAmp = 6, + kBlueSpeakerCenter = 7, + kBlueSpeakerOffset = 8, + kBlueChain1 = 15, + kBlueChain2 = 16, + kBlueChain3 = 14, + kBlueSourceLeft = 1, + kBlueSourceRight = 2, + + kRedAmp = 5, + kRedSpeakerCenter = 4, + kRedSpeakerOffset = 3, + + kRedChain1 = 12, + kRedChain2 = 11, + kRedChain3 = 13, + kRedSourceLeft = 9, + kRedSourceRight = 10 +}; + +enum class VisionModes { kAprilTag = 0, kRing = 1 }; + +enum class VisionTargetObjects { kNote }; + +struct AprilTag { + int id; + double size; + std::array, 4> transform; + bool unique; + + frc::Pose3d pos; + units::radian_t yaw; + units::radian_t pitch; + units::radian_t roll; +}; + +class FMAP { + public: + explicit FMAP(std::string path); + + std::vector GetTags(); + + private: + std::vector _tags; + std::string _path; + std::string deploy_dir; +}; + +class Vision { + public: + Vision(std::string name, FMAP fmap); + + std::pair GetDistanceToTarget(VisionTarget target); + std::pair GetDistanceToTarget(int id); + + void SetMode(VisionModes mode); + + frc::Pose3d GetPose(); + + frc::Pose2d AlignToTarget(VisionTarget target, units::meter_t offset, wom::SwerveDrive* swerveDrive); + frc::Pose2d AlignToTarget(int target, units::meter_t offset, wom::SwerveDrive* swerveDrive); + + frc::Pose2d AlignToTarget(VisionTarget target, frc::Translation2d offset, wom::SwerveDrive* swerveDrive); + frc::Pose2d AlignToTarget(int target, frc::Translation2d offset, wom::SwerveDrive* swerveDrive); + + frc::Pose2d TurnToTarget(int target, wom::SwerveDrive* swerveDrive); + frc::Pose2d TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive); + + std::pair GetAngleToObject(VisionTargetObjects object); + units::degree_t LockOn(VisionTargetObjects target); + + std::vector GetTags(); + + bool IsAtPose(frc::Pose3d pose, units::second_t dt); + bool IsAtPose(frc::Pose2d pose, units::second_t dt); + + bool TargetIsVisible(VisionTargetObjects target); + + int CurrentAprilTag(); + + wom::Limelight* GetLimelight(); + + private: + wom::Limelight* _limelight; + FMAP _fmap; +}; diff --git a/src/main/include/vision/VisionBehaviours.h b/src/main/include/vision/VisionBehaviours.h new file mode 100644 index 00000000..c10a092d --- /dev/null +++ b/src/main/include/vision/VisionBehaviours.h @@ -0,0 +1,56 @@ +// Copyright (c) 2023-2024 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + +#pragma once + +#include "Wombat.h" +#include "behaviour/HasBehaviour.h" +#include "drivetrain/SwerveDrive.h" +#include "units/length.h" +#include "units/time.h" +#include "vision/Vision.h" + +enum class VisionType { kLimelight = 0, kGenericCamera = 1 }; + +class AlignToAprilTag : public behaviour::Behaviour { + public: + explicit AlignToAprilTag(Vision* vision, VisionTarget target, wom::SwerveDrive* swerveDrive, + units::meter_t offset = 0_m); + + void OnTick(units::second_t dt); + + private: + Vision* _vision; + wom::SwerveDrive* _swerveDrive; + VisionTarget _target; + units::meter_t _offset; +}; + +class TurnToAprilTag : public behaviour::Behaviour { + public: + explicit TurnToAprilTag(Vision* vision, VisionTarget target, wom::SwerveDrive* swerveDrive); + + void OnTick(units::second_t dt); + + private: + Vision* _vision; + wom::SwerveDrive* _swerveDrive; + VisionTarget _target; +}; + +class LockOnToTarget : public behaviour::Behaviour { + public: + explicit LockOnToTarget(Vision* vision, VisionTargetObjects target, wom::SwerveDrive* swerveDrive); + explicit LockOnToTarget(wom::PhotonVision* camera, Vision* limelight, wom::SwerveDrive* swerveDrive); + + void OnTick(units::second_t dt); + + private: + Vision* _vision; + wom::PhotonVision* _camera; + + VisionTargetObjects _target; + wom::SwerveDrive* _swerveDrive; + VisionType _type; +}; diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json new file mode 100644 index 00000000..48b56188 --- /dev/null +++ b/vendordeps/PathplannerLib.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib.json", + "name": "PathplannerLib", + "version": "2024.2.3", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2024", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2024.2.3" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2024.2.3", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json new file mode 100644 index 00000000..67bf3898 --- /dev/null +++ b/vendordeps/WPILibNewCommands.json @@ -0,0 +1,38 @@ +{ + "fileName": "WPILibNewCommands.json", + "name": "WPILib-New-Commands", + "version": "1.0.0", + "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "frcYear": "2024", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-cpp", + "version": "wpilib", + "libName": "wpilibNewCommands", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxuniversal" + ] + } + ] +} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 00000000..6a4780c7 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,42 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2024.1.1-beta-3.2", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2024", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-cpp", + "version": "v2024.1.1-beta-3.2", + "libName": "Photon", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-java", + "version": "v2024.1.1-beta-3.2" + }, + { + "groupId": "org.photonvision", + "artifactId": "PhotonTargeting-java", + "version": "v2024.1.1-beta-3.2" + } + ] +} diff --git a/wombat/build.gradle b/wombat/build.gradle index a9b57267..7c616f9d 100644 --- a/wombat/build.gradle +++ b/wombat/build.gradle @@ -24,6 +24,29 @@ model { } binaries.all { + if (!project.hasProperty("opt")) { + def opt; + if (opt == null) { + opt = "release" + } + + if (!opt == "release") { + cppCompiler.define "DEBUG" + } else { + cppCompiler.define "RELEASE" + } + } else { + if (opt == null) { + opt = "release" + } + + if (!opt == "release") { + cppCompiler.define "DEBUG" + } else { + cppCompiler.define "RELEASE" + } + } + if (it.targetPlatform.name == "linuxathena") cppCompiler.define "PLATFORM_ROBORIO" else diff --git a/wombat/src/main/cpp/behaviour/Behaviour.cpp b/wombat/src/main/cpp/behaviour/Behaviour.cpp index d21d9e57..ae7ce73d 100644 --- a/wombat/src/main/cpp/behaviour/Behaviour.cpp +++ b/wombat/src/main/cpp/behaviour/Behaviour.cpp @@ -5,14 +5,13 @@ #include "behaviour/Behaviour.h" #include +#include using namespace behaviour; // Behaviour Behaviour::Behaviour(std::string name, units::time::second_t period) - : _bhvr_name(name), - _bhvr_period(period), - _bhvr_state(BehaviourState::INITIALISED) {} + : _bhvr_name(name), _bhvr_period(period), _bhvr_state(BehaviourState::INITIALISED) {} Behaviour::~Behaviour() { if (!IsFinished()) Interrupt(); @@ -82,8 +81,7 @@ bool Behaviour::Tick() { if (dt > 2 * _bhvr_period) { std::cerr << "Behaviour missed deadline. Reduce Period. Dt=" << dt.value() - << " Dt(deadline)=" << (2 * _bhvr_period).value() - << ". Bhvr: " << GetName() << std::endl; + << " Dt(deadline)=" << (2 * _bhvr_period).value() << ". Bhvr: " << GetName() << std::endl; } if (_bhvr_timeout.value() > 0 && _bhvr_timer > _bhvr_timeout) @@ -100,8 +98,7 @@ bool Behaviour::IsRunning() const { } bool Behaviour::IsFinished() const { - return _bhvr_state != BehaviourState::INITIALISED && - _bhvr_state != BehaviourState::RUNNING; + return _bhvr_state != BehaviourState::INITIALISED && _bhvr_state != BehaviourState::RUNNING; } void Behaviour::Stop(BehaviourState new_state) { @@ -132,7 +129,8 @@ void SequentialBehaviour::OnTick(units::time::second_t dt) { SetPeriod(_queue.front()->GetPeriod()); _queue.front()->Tick(); if (_queue.front()->IsFinished()) { - _queue.pop_front(); + // _queue.pop_front(); + _queue.erase(_queue.begin()); if (_queue.empty()) SetDone(); else @@ -147,11 +145,22 @@ void SequentialBehaviour::OnStop() { if (GetBehaviourState() != BehaviourState::DONE) { while (!_queue.empty()) { _queue.front()->Interrupt(); - _queue.pop_front(); + // _queue.pop_front(); + _queue.erase(_queue.begin()); } } } +std::vector SequentialBehaviour::GetQueue() { + std::vector names; + + for (auto bh : _queue) { + names.push_back(bh->GetName()); + } + + return names; +} + // ConcurrentBehaviour ConcurrentBehaviour::ConcurrentBehaviour(ConcurrentBehaviourReducer reducer) : Behaviour(), _reducer(reducer) {} @@ -173,8 +182,7 @@ void ConcurrentBehaviour::Add(Behaviour::ptr behaviour) { } std::string ConcurrentBehaviour::GetName() const { - std::string msg = - (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {"); + std::string msg = (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {"); for (auto b : _children) msg += b->GetName() + ", "; msg += "}"; @@ -190,8 +198,8 @@ void ConcurrentBehaviour::OnStart() { using namespace std::chrono_literals; b->Tick(); - std::this_thread::sleep_for(std::chrono::milliseconds( - static_cast(b->GetPeriod().value() * 1000))); + std::this_thread::sleep_for( + std::chrono::milliseconds(static_cast(b->GetPeriod().value() * 1000))); } if (IsFinished() && !b->IsFinished()) @@ -234,6 +242,15 @@ void ConcurrentBehaviour::OnStop() { } } +std::vector ConcurrentBehaviour::GetQueue() { + std::vector vec; + + for (auto beh : _children) { + vec.push_back(beh->GetName()); + } + + return vec; +} // If If::If(std::function condition) : _condition(condition) {} If::If(bool v) : _condition([v]() { return v; }) {} @@ -273,10 +290,8 @@ void WaitFor::OnTick(units::time::second_t dt) { } // WaitTime -WaitTime::WaitTime(units::time::second_t time) - : WaitTime([time]() { return time; }) {} -WaitTime::WaitTime(std::function time_fn) - : _time_fn(time_fn) {} +WaitTime::WaitTime(units::time::second_t time) : WaitTime([time]() { return time; }) {} +WaitTime::WaitTime(std::function time_fn) : _time_fn(time_fn) {} void WaitTime::OnStart() { _time = _time_fn(); diff --git a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp index 26863c79..eb363b3e 100644 --- a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp +++ b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp @@ -52,8 +52,8 @@ void BehaviourScheduler::Schedule(Behaviour::ptr behaviour) { std::lock_guard lk(_active_mtx); behaviour->Tick(); } - std::this_thread::sleep_for(std::chrono::milliseconds( - static_cast(behaviour->GetPeriod().value() * 1000))); + std::this_thread::sleep_for( + std::chrono::milliseconds(static_cast(behaviour->GetPeriod().value() * 1000))); } }); } diff --git a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp index 964522c6..f2f557c5 100644 --- a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp +++ b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp @@ -8,8 +8,7 @@ using namespace behaviour; -void HasBehaviour::SetDefaultBehaviour( - std::function(void)> fn) { +void HasBehaviour::SetDefaultBehaviour(std::function(void)> fn) { _default_behaviour_producer = fn; } diff --git a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp deleted file mode 100644 index e516e7d7..00000000 --- a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp +++ /dev/null @@ -1,52 +0,0 @@ -// Copyright (c) 2023-2024 CurtinFRC -// Open Source Software, you can modify it according to the terms -// of the MIT License at the root of this project - -#include "drivetrain/Drivetrain.h" - -#include - -#include "utils/Util.h" - -using namespace frc; -using namespace units; - -wom::drivetrain::Drivetrain::Drivetrain(DrivetrainConfig* config, - XboxController& driver) - : _config(config), _driver(driver) {} -wom::drivetrain::Drivetrain::~Drivetrain() {} - -wom::drivetrain::DrivetrainConfig* wom::drivetrain::Drivetrain::GetConfig() { - return _config; -} -wom::drivetrain::DrivetrainState wom::drivetrain::Drivetrain::GetState() { - return _state; -} - -void wom::drivetrain::Drivetrain::SetState(DrivetrainState state) { - _state = state; -} - -void wom::drivetrain::Drivetrain::OnStart() { - std::cout << "Starting Tank" << std::endl; -} - -void wom::drivetrain::Drivetrain::OnUpdate(second_t dt) { - switch (_state) { - case DrivetrainState::kIdle: - break; - case DrivetrainState::kTank: { - double rightSpeed = wom::utils::deadzone(_driver.GetRightY()); - double leftSpeed = wom::utils::deadzone(_driver.GetLeftY()); - // _config->left1.motorController->SetVoltage(leftSpeed * maxVolts); - // _config->left2.motorController->SetVoltage(leftSpeed * maxVolts); - // _config->left3.motorController->SetVoltage(leftSpeed * maxVolts); - // _config->right1.motorController->SetVoltage(rightSpeed * maxVolts); - // _config->right2.motorController->SetVoltage(rightSpeed * maxVolts); - // _config->right3.motorController->SetVoltage(rightSpeed * maxVolts); - break; - } - case DrivetrainState::kAuto: - break; - } -} diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp index 773d5705..6e434724 100644 --- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp +++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp @@ -5,45 +5,38 @@ #include "drivetrain/SwerveDrive.h" #include +#include #include #include -#include - -#include "utils/Util.h" - -#include -#include -#include - #include #include +#include +#include +#include + #include "frc/MathUtil.h" -#include "wpimath/MathShared.h" +#include "units/velocity.h" +#include "utils/Util.h" using namespace wom; namespace wom { namespace drivetrain { - -PIDController::PIDController(double Kp, double Ki, double Kd, - units::second_t period) +PIDController::PIDController(double Kp, double Ki, double Kd, units::second_t period) : m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) { bool invalidGains = false; if (Kp < 0.0) { - wpi::math::MathSharedStore::ReportError( - "Kp must be a non-negative number, got {}!", Kp); + wpi::math::MathSharedStore::ReportError("Kp must be a non-negative number, got {}!", Kp); invalidGains = true; } if (Ki < 0.0) { - wpi::math::MathSharedStore::ReportError( - "Ki must be a non-negative number, got {}!", Ki); + wpi::math::MathSharedStore::ReportError("Ki must be a non-negative number, got {}!", Ki); invalidGains = true; } if (Kd < 0.0) { - wpi::math::MathSharedStore::ReportError( - "Kd must be a non-negative number, got {}!", Kd); + wpi::math::MathSharedStore::ReportError("Kd must be a non-negative number, got {}!", Kd); invalidGains = true; } if (invalidGains) { @@ -54,17 +47,15 @@ PIDController::PIDController(double Kp, double Ki, double Kd, } if (period <= 0_s) { - wpi::math::MathSharedStore::ReportError( - "Controller period must be a positive number, got {}!", period.value()); + wpi::math::MathSharedStore::ReportError("Controller period must be a positive number, got {}!", + period.value()); m_period = 20_ms; - wpi::math::MathSharedStore::ReportWarning( - "Controller period defaulted to 20ms."); + wpi::math::MathSharedStore::ReportWarning("Controller period defaulted to 20ms."); } static int instances = 0; instances++; - wpi::math::MathSharedStore::ReportUsage( - wpi::math::MathUsageId::kController_PIDController2, instances); + wpi::math::MathSharedStore::ReportUsage(wpi::math::MathUsageId::kController_PIDController2, instances); wpi::SendableRegistry::Add(this, "PIDController", instances); } @@ -88,8 +79,7 @@ void PIDController::SetD(double Kd) { void PIDController::SetIZone(double iZone) { if (iZone < 0) { - wpi::math::MathSharedStore::ReportError( - "IZone must be a non-negative number, got {}!", iZone); + wpi::math::MathSharedStore::ReportError("IZone must be a non-negative number, got {}!", iZone); } m_iZone = iZone; } @@ -128,8 +118,7 @@ void PIDController::SetSetpoint(double setpoint) { if (m_continuous) { double errorBound = (m_maximumInput - m_minimumInput) / 2.0; - m_positionError = - frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound); + m_positionError = frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound); } else { m_positionError = m_setpoint - m_measurement; } @@ -142,13 +131,11 @@ double PIDController::GetSetpoint() const { } bool PIDController::AtSetpoint() const { - return m_haveMeasurement && m_haveSetpoint && - std::abs(m_positionError) < m_positionTolerance && + return m_haveMeasurement && m_haveSetpoint && std::abs(m_positionError) < m_positionTolerance && std::abs(m_velocityError) < m_velocityTolerance; } -void PIDController::EnableContinuousInput(double minimumInput, - double maximumInput) { +void PIDController::EnableContinuousInput(double minimumInput, double maximumInput) { m_continuous = true; m_minimumInput = minimumInput; m_maximumInput = maximumInput; @@ -162,14 +149,12 @@ bool PIDController::IsContinuousInputEnabled() const { return m_continuous; } -void PIDController::SetIntegratorRange(double minimumIntegral, - double maximumIntegral) { +void PIDController::SetIntegratorRange(double minimumIntegral, double maximumIntegral) { m_minimumIntegral = minimumIntegral; m_maximumIntegral = maximumIntegral; } -void PIDController::SetTolerance(double positionTolerance, - double velocityTolerance) { +void PIDController::SetTolerance(double positionTolerance, double velocityTolerance) { m_positionTolerance = positionTolerance; m_velocityTolerance = velocityTolerance; } @@ -189,8 +174,7 @@ double PIDController::Calculate(double measurement) { if (m_continuous) { double errorBound = (m_maximumInput - m_minimumInput) / 2.0; - m_positionError = - frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound); + m_positionError = frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound); } else { m_positionError = m_setpoint - m_measurement; } @@ -202,9 +186,8 @@ double PIDController::Calculate(double measurement) { if (std::abs(m_positionError) > m_iZone) { m_totalError = 0; } else if (m_Ki != 0) { - m_totalError = - std::clamp(m_totalError + m_positionError * m_period.value(), - m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki); + m_totalError = std::clamp(m_totalError + m_positionError * m_period.value(), m_minimumIntegral / m_Ki, + m_maximumIntegral / m_Ki); } // double absError = m_setpoint - m_measurement; @@ -240,41 +223,37 @@ void PIDController::InitSendable(wpi::SendableBuilder& builder) { builder.AddDoubleProperty( "d", [this] { return GetD(); }, [this](double value) { SetD(value); }); builder.AddDoubleProperty( - "izone", [this] { return GetIZone(); }, - [this](double value) { SetIZone(value); }); + "izone", [this] { return GetIZone(); }, [this](double value) { SetIZone(value); }); builder.AddDoubleProperty( - "setpoint", [this] { return GetSetpoint(); }, - [this](double value) { SetSetpoint(value); }); + "setpoint", [this] { return GetSetpoint(); }, [this](double value) { SetSetpoint(value); }); } - - - -void SwerveModuleConfig::WriteNT( - std::shared_ptr table) const { +void SwerveModuleConfig::WriteNT(std::shared_ptr table) const { + #ifdef DEBUG std::array pos{position.X().value(), position.Y().value()}; table->GetEntry("position").SetDoubleArray(std::span(pos)); table->GetEntry("wheelRadius").SetDouble(wheelRadius.value()); + #endif } SwerveModule::SwerveModule(std::string path, SwerveModuleConfig config, - //SwerveModule::angle_pid_conf_t anglePID, + // SwerveModule::angle_pid_conf_t anglePID, SwerveModule::velocity_pid_conf_t velocityPID) - : //_anglePIDController(path + "/pid/angle", anglePID), + : //_anglePIDController(path + "/pid/angle", anglePID), _anglePIDController{frc::PIDController(5, 0, 0, 0.005_s)}, _config(config), _velocityPIDController(frc::PIDController(1.2, 0, 0, 0.005_s)), _table(nt::NetworkTableInstance::GetDefault().GetTable(path)) { // _anglePIDController.SetTolerance(360); // _anglePIDController.EnableContinuousInput(-3.1415, (2 * 3.1415)); - _anglePIDController.EnableContinuousInput(0, (2*3.1415)); + _anglePIDController.EnableContinuousInput(0, (2 * 3.1415)); } void SwerveModule::OnStart() { // _offset = offset; // _config.canEncoder->SetPosition(units::turn_t{0}); _anglePIDController.Reset(); - _anglePIDController.EnableContinuousInput(0, 2*3.14159); + _anglePIDController.EnableContinuousInput(0, 2 * 3.14159); _velocityPIDController.Reset(); } @@ -282,8 +261,6 @@ void SwerveModule::OnUpdate(units::second_t dt) { units::volt_t driveVoltage{0}; units::volt_t turnVoltage{0}; - - switch (_state) { case SwerveModuleState::kIdle: driveVoltage = 0_V; @@ -291,17 +268,19 @@ void SwerveModule::OnUpdate(units::second_t dt) { break; case SwerveModuleState::kPID: { auto feedforward = _config.driveMotor.motor.Voltage( - 0_Nm, units::radians_per_second_t{ - _velocityPIDController.GetSetpoint() / _config.wheelRadius.value()}); - // units::radian_t input = units::math::fmod(_config.turnMotor.encoder->GetEncoderPosition(), (2_rad * 3.1415)); + 0_Nm, + units::radians_per_second_t{_velocityPIDController.GetSetpoint() / _config.wheelRadius.value()}); + // units::radian_t input = units::math::fmod(_config.turnMotor.encoder->GetEncoderPosition(), (2_rad + // * 3.1415)); double input = _config.turnMotor.encoder->GetEncoderPosition().value(); + #ifdef DEBUG _table->GetEntry("/testing/GetEncoderPos").SetDouble(input); + #endif // _velocityPIDController.SetSetpoint(3); - driveVoltage = units::volt_t{_velocityPIDController.Calculate(GetSpeed().value())}; // if (_turnOffset == TurnOffsetValues::forward) { - + // } else if (_turnOffset == TurnOffsetValues::reverse) { // input = input - (3.1415/2); // driveVoltage = -driveVoltage; @@ -318,8 +297,7 @@ void SwerveModule::OnUpdate(units::second_t dt) { std::cerr << "Case not handled" << std::endl; } - units::newton_meter_t torqueLimit = - 50_kg / 4 * _config.wheelRadius * _currentAccelerationLimit; + units::newton_meter_t torqueLimit = 50_kg / 4 * _config.wheelRadius * _currentAccelerationLimit; // units::volt_t voltageMax = _config.driveMotor.motor.Voltage( // torqueLimit, _config.driveMotor.encoder->GetEncoderAngularVelocity()); // units::volt_t voltageMin = _config.driveMotor.motor.Voltage( @@ -329,27 +307,30 @@ void SwerveModule::OnUpdate(units::second_t dt) { // units::math::max(units::math::min(driveVoltage, voltageMax), voltageMin); driveVoltage = units::math::min(driveVoltage, 7_V); - turnVoltage = units::math::min(turnVoltage, 4_V); + turnVoltage = units::math::min(turnVoltage, 6_V); // driveVoltage = units::math::min( // units::math::max(driveVoltage, -_driveModuleVoltageLimit), // _driveModuleVoltageLimit); // was originally 10_V units::volt_t turnVoltageMax = 7_V - (driveVoltage * (7_V / 10_V)); - turnVoltage = units::math::min(units::math::max(turnVoltage, -turnVoltageMax), - turnVoltageMax); + turnVoltage = units::math::min(units::math::max(turnVoltage, -turnVoltageMax), turnVoltageMax); // turnVoltage = units::math::min(units::math::max(turnVoltage, -7_V), 7_V); + #ifdef DEBUG _table->GetEntry("TurnVoltage").SetDouble(turnVoltage.value()); _table->GetEntry("TurnSetpoint").SetDouble(_anglePIDController.GetSetpoint()); _table->GetEntry("Demand").SetDouble(_config.turnMotor.encoder->GetEncoderPosition().value()); _table->GetEntry("Error").SetDouble(_anglePIDController.GetPositionError()); + #endif _config.driveMotor.motorController->SetVoltage(driveVoltage); _config.turnMotor.motorController->SetVoltage(turnVoltage); + #ifdef DEBUG _table->GetEntry("speed").SetDouble(GetSpeed().value()); _table->GetEntry("angle").SetDouble(_config.turnMotor.encoder->GetEncoderPosition().value()); _config.WriteNT(_table->GetSubTable("config")); + #endif } void SwerveModule::SetTurnOffsetForward() { @@ -368,13 +349,11 @@ void SwerveModule::TurnOffset() { // return (_config.turnMotor.encoder->GetEncoderPosition().value()); // } -void SwerveModule::SetAccelerationLimit( - units::meters_per_second_squared_t limit) { +void SwerveModule::SetAccelerationLimit(units::meters_per_second_squared_t limit) { _currentAccelerationLimit = limit; } -void SwerveDrive::SetAccelerationLimit( - units::meters_per_second_squared_t limit) { +void SwerveDrive::SetAccelerationLimit(units::meters_per_second_squared_t limit) { for (int motorNumber = 0; motorNumber < 4; motorNumber++) { _modules[motorNumber].SetAccelerationLimit(limit); } @@ -393,12 +372,9 @@ void SwerveModule::SetZero(units::second_t dt) { _state = SwerveModuleState::kPID; } -void SwerveModule::SetPID(units::radian_t angle, - units::meters_per_second_t speed, - units::second_t dt) { +void SwerveModule::SetPID(units::radian_t angle, units::meters_per_second_t speed, units::second_t dt) { _state = SwerveModuleState::kPID; - // double diff = std::abs(_config.turnMotor.encoder->GetEncoderPosition().value() - angle.value()); // _table->GetEntry("diff").SetDouble(diff); // if (std::abs(diff) > (3.14159/2)) { @@ -407,45 +383,38 @@ void SwerveModule::SetPID(units::radian_t angle, // _anglePIDController.SetSetpoint(angle.value()); // _velocityPIDController.SetSetpoint(speed.value()); // } else { - _anglePIDController.SetSetpoint(angle.value()); - _velocityPIDController.SetSetpoint(speed.value()); + _anglePIDController.SetSetpoint(angle.value()); + _velocityPIDController.SetSetpoint(speed.value()); // } } void SwerveModule::ModuleVectorHandler(frc::ChassisSpeeds speeds) { units::meters_per_second_t xVelocityComponent = - 1_mps * - (speeds.vx.value() + speeds.omega.value() * _config.position.X().value()); + 1_mps * (speeds.vx.value() + speeds.omega.value() * _config.position.X().value()); units::meters_per_second_t yVelocityComponent = - 1_mps * - (speeds.vy.value() + speeds.omega.value() * _config.position.Y().value()); + 1_mps * (speeds.vy.value() + speeds.omega.value() * _config.position.Y().value()); units::meters_per_second_t velocity = - 1_mps * std::sqrt(std::pow(xVelocityComponent.value(), 2) + - std::pow(yVelocityComponent.value(), 2)); - units::degree_t angle = 1_rad * std::atan2(yVelocityComponent.value(), - xVelocityComponent.value()); + 1_mps * std::sqrt(std::pow(xVelocityComponent.value(), 2) + std::pow(yVelocityComponent.value(), 2)); + units::radian_t angle = 1_rad * std::atan2(yVelocityComponent.value(), xVelocityComponent.value()); _anglePIDController.SetSetpoint(angle.value()); _velocityPIDController.SetSetpoint(velocity.value()); } units::meters_per_second_t SwerveModule::GetSpeed() const { - units::meters_per_second_t returnVal{ - _config.driveMotor.encoder->GetVelocityValue() * - _config.wheelRadius.value()}; - return returnVal; + units::meters_per_second_t returnVal{_config.driveMotor.encoder->GetVelocityValue() * + _config.wheelRadius.value()}; + return returnVal; } units::meter_t SwerveModule::GetDistance() const { - return units::meter_t{ - _config.driveMotor.encoder->GetEncoderPosition().value() * - _config.wheelRadius.value()}; + return units::meter_t{_config.driveMotor.encoder->GetEncoderPosition().value() * + _config.wheelRadius.value()}; } frc::SwerveModulePosition SwerveModule::GetPosition() const { - return frc::SwerveModulePosition{ - GetDistance(), _config.turnMotor.encoder->GetEncoderPosition()}; + return frc::SwerveModulePosition{GetDistance(), _config.turnMotor.encoder->GetEncoderPosition()}; } const SwerveModuleConfig& SwerveModule::GetConfig() const { @@ -453,26 +422,33 @@ const SwerveModuleConfig& SwerveModule::GetConfig() const { } void SwerveDriveConfig::WriteNT(std::shared_ptr table) { + #ifdef DEBUG table->GetEntry("mass").SetDouble(mass.value()); + #endif } SwerveDrive::SwerveDrive(SwerveDriveConfig config, frc::Pose2d initialPose) : _config(config), - _kinematics(_config.modules[3].position, _config.modules[0].position, - _config.modules[1].position, _config.modules[2].position), + _kinematics(_config.modules[3].position, _config.modules[0].position, _config.modules[1].position, + _config.modules[2].position), _poseEstimator( _kinematics, frc::Rotation2d(0_deg), - wpi::array{ - frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}, - frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}, - frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}, - frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}}, + wpi::array{frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}, + frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}, + frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}, + frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}}, initialPose, _config.stateStdDevs, _config.visionMeasurementStdDevs), - _anglePIDController{PIDController(1, 0, 0)}, - _xPIDController(config.path + "/pid/x", _config.posePositionPID), - _yPIDController(config.path + "/pid/y", _config.posePositionPID), + _anglePIDController{PIDController(0, 0, 0)}, + _xPIDController(PIDController(4, 0.1, 0)), + _yPIDController(PIDController(4, 0.1, 0)), + _turnPIDController(PIDController(7, 0, 0)), + // _xPIDController(std::string path, config_t initialGains) + // _xPIDController(config.path + "/pid/x", _config.posePositionPID), + // _yPIDController(config.path + "/pid/y", _config.posePositionPID), _table(nt::NetworkTableInstance::GetDefault().GetTable(_config.path)) { _anglePIDController.SetTolerance(360); + _anglePIDController.EnableContinuousInput(0, 2 * 3.14159); + // _anglePIDController.EnableContinuousInput(-3.14159, 3.14159); int i = 1; for (auto cfg : _config.modules) { @@ -484,16 +460,16 @@ SwerveDrive::SwerveDrive(SwerveDriveConfig config, frc::Pose2d initialPose) ResetPose(initialPose); } -frc::ChassisSpeeds FieldRelativeSpeeds::ToChassisSpeeds( - const units::radian_t robotHeading) { - return frc::ChassisSpeeds::FromFieldRelativeSpeeds( - vx, vy, omega, frc::Rotation2d{robotHeading}); +frc::ChassisSpeeds FieldRelativeSpeeds::ToChassisSpeeds(const units::radian_t robotHeading) { + return frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, frc::Rotation2d{robotHeading}); } void SwerveDrive::OnUpdate(units::second_t dt) { + #ifdef DEBUG _table->GetEntry("/gryo/z").SetDouble(_config.gyro->GetRotation3d().Z().value()); _table->GetEntry("/gryo/y").SetDouble(_config.gyro->GetRotation3d().Y().value()); _table->GetEntry("/gryo/x").SetDouble(_config.gyro->GetRotation3d().X().value()); + #endif switch (_state) { case SwerveDriveState::kZeroing: for (auto mod = _modules.begin(); mod < _modules.end(); mod++) { @@ -505,24 +481,65 @@ void SwerveDrive::OnUpdate(units::second_t dt) { mod->SetIdle(); } break; + + case SwerveDriveState::kAngle: { + _turnPIDController.SetSetpoint(_reqAngle.value()); + + double direction = _turnPIDController.Calculate(_config.gyro->GetRotation2d().Radians().value()); + + for (size_t i = 0; i < _modules.size(); i++) { + switch (i) { + case 0: { + _modules[i].SetPID(135_deg, units::meters_per_second_t{-direction}, dt); + } break; + + case 1: { + _modules[i].SetPID(45_deg, units::meters_per_second_t{-direction}, dt); + } break; + + case 2: { + _modules[i].SetPID(135_deg, units::meters_per_second_t{direction}, dt); + } break; + + case 3: { + _modules[i].SetPID(45_deg, units::meters_per_second_t{direction}, dt); + } break; + } + } + + #ifdef DEBUG + _table->GetEntry("direction").SetDouble(direction); + _table->GetEntry("_reqAngle").SetDouble(_reqAngle.value()); +#endif + } break; case SwerveDriveState::kPose: { - _target_fr_speeds.vx = _xPIDController.Calculate(GetPose().X(), dt); - _target_fr_speeds.vy = _yPIDController.Calculate(GetPose().Y(), dt); + // _target_fr_speeds.vx = _xPIDController.Calculate(GetPose().X(), dt); + // _target_fr_speeds.vy = _yPIDController.Calculate(GetPose().Y(), dt); + _target_fr_speeds.vx = units::meters_per_second_t{_xPIDController.Calculate(GetPose().X().value())}; + _target_fr_speeds.vy = units::meters_per_second_t{_yPIDController.Calculate(GetPose().Y().value())}; _target_fr_speeds.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())}; + + #ifdef DEBUG + _table->GetEntry("Swerve vx").SetDouble(_target_fr_speeds.vx.value()); + _table->GetEntry("Swerve vy").SetDouble(_target_fr_speeds.vy.value()); + _table->GetEntry("Swerve omega").SetDouble(_target_fr_speeds.omega.value()); +#endif } [[fallthrough]]; case SwerveDriveState::kFieldRelativeVelocity: - _target_speed = - _target_fr_speeds.ToChassisSpeeds(GetPose().Rotation().Radians()); + _target_speed = _target_fr_speeds.ToChassisSpeeds(GetPose().Rotation().Radians()); if (isRotateToMatchJoystick) { - _target_speed.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())}; + _target_speed.omega = units::radians_per_second_t{ + _anglePIDController.Calculate(GetPose().Rotation().Radians().value())}; } // _target_fr_speeds.vy.value() << std::endl; [[fallthrough]]; case SwerveDriveState::kVelocity: { + #ifdef DEBUG _table->GetEntry("Swerve module VX").SetDouble(_target_speed.vx.value()); _table->GetEntry("Swerve module VY").SetDouble(_target_speed.vy.value()); _table->GetEntry("Swerve module Omega").SetDouble(_target_speed.omega.value()); +#endif if (_target_speed.omega.value() > 0) { // _modules[0].SetTurnOffsetForward(); _modules[1].SetTurnOffsetForward(); @@ -538,10 +555,7 @@ void SwerveDrive::OnUpdate(units::second_t dt) { _angle = _target_speed.omega * 1_s; } - - bool init = false; - - frc::ChassisSpeeds new_target_speed {_target_speed.vx, _target_speed.vy, -_target_speed.omega}; + frc::ChassisSpeeds new_target_speed{_target_speed.vx, _target_speed.vy, -_target_speed.omega}; auto target_states = _kinematics.ToSwerveModuleStates(_target_speed); auto new_target_states = _kinematics.ToSwerveModuleStates(new_target_speed); for (size_t i = 0; i < _modules.size(); i++) { @@ -550,7 +564,6 @@ void SwerveDrive::OnUpdate(units::second_t dt) { } else { _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt); } - } } break; case SwerveDriveState::kIndividualTuning: @@ -569,15 +582,17 @@ void SwerveDrive::OnUpdate(units::second_t dt) { _modules[3].SetPID(315_deg, 0_mps, dt); break; case SwerveDriveState::kFRVelocityRotationLock: - _target_speed.vx = _xPIDController.Calculate(GetPose().X(), dt); - _target_speed.vy = _yPIDController.Calculate(GetPose().Y(), dt); - _target_speed.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())}; - _target_speed = - _requestedSpeeds.ToChassisSpeeds(GetPose().Rotation().Radians()); + // _target_speed.vx = _xPIDController.Calculate(GetPose().X(), dt); + // _target_speed.vy = _yPIDController.Calculate(GetPose().Y(), dt); + _target_fr_speeds.vx = units::meters_per_second_t{_xPIDController.Calculate(GetPose().X().value())}; + _target_fr_speeds.vx = units::meters_per_second_t{_xPIDController.Calculate(GetPose().Y().value())}; + + _target_speed.omega = + units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())}; + _target_speed = _requestedSpeeds.ToChassisSpeeds(GetPose().Rotation().Radians()); auto target_states = _kinematics.ToSwerveModuleStates(_target_speed); for (size_t i = 0; i < _modules.size(); i++) { - _modules[i].SetPID(target_states[i].angle.Radians(), - target_states[i].speed, dt); + _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt); } break; } @@ -587,13 +602,11 @@ void SwerveDrive::OnUpdate(units::second_t dt) { } _poseEstimator.Update( - _config.gyro->GetRotation2d(), - wpi::array{ - _modules[3].GetPosition(), _modules[0].GetPosition(), - _modules[1].GetPosition(), _modules[2].GetPosition()}); + _config.gyro->GetRotation2d() /* - 1_rad*/, + wpi::array{_modules[3].GetPosition(), _modules[0].GetPosition(), + _modules[1].GetPosition(), _modules[2].GetPosition()}); - utils::WritePose2NT(_table->GetSubTable("estimatedPose"), - _poseEstimator.GetEstimatedPosition()); + utils::WritePose2NT(_table->GetSubTable("estimatedPose"), _poseEstimator.GetEstimatedPosition()); _config.WriteNT(_table->GetSubTable("config")); } @@ -618,6 +631,9 @@ void SwerveDrive::SetVoltageLimit(units::volt_t driveVoltageLimit) { void SwerveDrive::OnStart() { OnResetMode(); + _anglePIDController.EnableContinuousInput(0, 2 * 3.14159); + // _anglePIDController.EnableContinuousInput(-3.14159, 3.14159); + _modules[0].OnStart(); // front left _modules[1].OnStart(); // front right _modules[2].OnStart(); // back right @@ -630,8 +646,7 @@ void SwerveDrive::OnResetMode() { _anglePIDController.Reset(); } -void SwerveDrive::RotateMatchJoystick(units::radian_t joystickAngle, - FieldRelativeSpeeds speeds) { +void SwerveDrive::RotateMatchJoystick(units::radian_t joystickAngle, FieldRelativeSpeeds speeds) { // _state = SwerveDriveState::kFRVelocityRotationLock; // _anglePIDController.SetSetpoint(joystickAngle); // _target_fr_speeds = speeds; @@ -657,16 +672,14 @@ bool SwerveDrive::GetIsFieldRelative() { return _isFieldRelative; } -void SwerveDrive::SetIndividualTuning(int mod, units::radian_t angle, - units::meters_per_second_t speed) { +void SwerveDrive::SetIndividualTuning(int mod, units::radian_t angle, units::meters_per_second_t speed) { _mod = mod; _angle = angle; _speed = speed; _state = SwerveDriveState::kIndividualTuning; } -void SwerveDrive::SetTuning(units::radian_t angle, - units::meters_per_second_t speed) { +void SwerveDrive::SetTuning(units::radian_t angle, units::meters_per_second_t speed) { _angle = angle; _speed = speed; _state = SwerveDriveState::kTuning; @@ -680,32 +693,82 @@ void SwerveDrive::SetFieldRelativeVelocity(FieldRelativeSpeeds speeds) { void SwerveDrive::SetPose(frc::Pose2d pose) { _state = SwerveDriveState::kPose; - _anglePIDController.SetSetpoint(pose.Rotation().Radians().value()); - _xPIDController.SetSetpoint(pose.X()); - _yPIDController.SetSetpoint(pose.Y()); + if (pose.X() > 4_m) { + pose = frc::Pose2d(0_m, pose.Y(), pose.Rotation()); + } + if (pose.Y() > 4_m) { + pose = frc::Pose2d(pose.X(), 0_m, pose.Rotation()); + } + + if (pose.X() < -4_m) { + pose = frc::Pose2d(0_m, pose.Y(), pose.Rotation()); + } + if (pose.Y() < -4_m) { + pose = frc::Pose2d(pose.X(), 0_m, pose.Rotation()); + } + _anglePIDController.SetSetpoint(pose.Rotation().Radians().value()/* - 3.14159*/); + _xPIDController.SetSetpoint(pose.X().value()); + _yPIDController.SetSetpoint(pose.Y().value()); + utils::WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("swerveSetpoint"), GetSetpoint()); } bool SwerveDrive::IsAtSetPose() { - return /*_anglePIDController.IsStable()*/ true && _xPIDController.IsStable() && - _yPIDController.IsStable(0.05_m); + if (std::abs(_xPIDController.GetPositionError()) < 0.1 && + std::abs(_yPIDController.GetPositionError()) < 0.1) { + if (std::abs(_xPIDController.GetVelocityError()) < 1 && + std::abs(_yPIDController.GetVelocityError()) < 1) { + return false; + } + } + return false; +} + +bool SwerveDrive::IsAtSetAngle() { + if (std::abs(_turnPIDController.GetPositionError()) < 0.1 && + std::abs(_turnPIDController.GetVelocityError()) < 0.1) { + return true; + } + return false; +} + +SwerveDriveState SwerveDrive::GetState() { + return _state; } void SwerveDrive::ResetPose(frc::Pose2d pose) { + #ifdef DEBUG + std::cout << "RESETTING POSE" << std::endl; + #endif _poseEstimator.ResetPosition( - _config.gyro->GetRotation2d(), - wpi::array{ - _modules[0].GetPosition(), _modules[1].GetPosition(), - _modules[2].GetPosition(), _modules[3].GetPosition()}, - pose); + _config.gyro->GetRotation2d() /* - 1_rad*/, + wpi::array{_modules[3].GetPosition(), _modules[0].GetPosition(), + _modules[1].GetPosition(), _modules[2].GetPosition()}, + frc::Pose2d(0_m, 0_m, 0_deg)); } frc::Pose2d SwerveDrive::GetPose() { - return _poseEstimator.GetEstimatedPosition(); + return frc::Pose2d(_poseEstimator.GetEstimatedPosition().X(), _poseEstimator.GetEstimatedPosition().Y(), + _poseEstimator.GetEstimatedPosition().Rotation()); + // return frc::Pose2d(1_m, 1_m, 0_deg); +} + +void SwerveDrive::AddVisionMeasurement(frc::Pose2d pose, units::second_t timestamp) { + // _poseEstimator.AddVisionMeasurement(pose, timestamp); +} + +frc::Pose2d SwerveDrive::GetSetpoint() { + return frc::Pose2d(units::meter_t{_xPIDController.GetSetpoint()}, + units::meter_t{_yPIDController.GetSetpoint()}, + units::radian_t{_anglePIDController.GetSetpoint()}); +} + +void SwerveDrive::MakeAtSetPoint() { + ResetPose(GetSetpoint()); } -void SwerveDrive::AddVisionMeasurement(frc::Pose2d pose, - units::second_t timestamp) { - _poseEstimator.AddVisionMeasurement(pose, timestamp); +void SwerveDrive::TurnToAngle(units::radian_t angle) { + _reqAngle = angle; + _state = SwerveDriveState::kAngle; } } // namespace drivetrain } // namespace wom diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp index 7e21760f..bb792d00 100644 --- a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp +++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp @@ -12,6 +12,9 @@ #include #include +#include "drivetrain/SwerveDrive.h" +#include "networktables/NetworkTableInstance.h" +#include "units/angle.h" #include "utils/Pathplanner.h" #include "utils/Util.h" @@ -72,45 +75,41 @@ void ManualDrivebase::OnTick(units::second_t deltaTime) { // if (isZero) { // _swerveDrivebase->SetZeroing(); // } else { - double xVelocity = wom::utils::spow2(-wom::utils::deadzone( - _driverController->GetLeftY(), - driverDeadzone)); // GetLeftY due to x being where y should be on field - double yVelocity = wom::utils::spow2( - -wom::utils::deadzone(_driverController->GetLeftX(), driverDeadzone)); - - double r_x = wom::utils::spow2( - -wom::utils::deadzone(_driverController->GetRightX(), turningDeadzone)); - - double turnX = _driverController->GetRightX(); - double turnY = _driverController->GetRightY(); - double num = std::sqrt(turnX * turnX + turnY * turnY); - if (num < turningDeadzone) { - turnX = 0; - turnY = 0; - } + double xVelocity = wom::utils::spow2( + -wom::utils::deadzone(_driverController->GetLeftY(), + driverDeadzone)); // GetLeftY due to x being where y should be on field + double yVelocity = wom::utils::spow2(-wom::utils::deadzone(_driverController->GetLeftX(), driverDeadzone)); + + double r_x = wom::utils::spow2(-wom::utils::deadzone(_driverController->GetRightX(), turningDeadzone)); + + double turnX = _driverController->GetRightX(); + double turnY = _driverController->GetRightY(); + double num = std::sqrt(turnX * turnX + turnY * turnY); + if (num < turningDeadzone) { + turnX = 0; + turnY = 0; + } + + // if (isRotateMatch) { + // units::degree_t currentAngle = + // _swerveDrivebase->GetPose().Rotation().Degrees(); + // CalculateRequestedAngle(turnX, turnY, currentAngle); + // _swerveDriveTable->GetEntry("RotateMatch") + // .SetDouble(_requestedAngle.value()); + // _swerveDrivebase->RotateMatchJoystick( + // _requestedAngle, + // wom::drivetrain::FieldRelativeSpeeds{// also field relative + // xVelocity * maxMovementMagnitude, + // yVelocity * maxMovementMagnitude, + // r_x * maxRotationMagnitude}); + // } else { + _swerveDrivebase->SetFieldRelativeVelocity(wom::drivetrain::FieldRelativeSpeeds{ + xVelocity * -maxMovementMagnitude, yVelocity * -maxMovementMagnitude, r_x * -maxRotationMagnitude}); - // if (isRotateMatch) { - // units::degree_t currentAngle = - // _swerveDrivebase->GetPose().Rotation().Degrees(); - // CalculateRequestedAngle(turnX, turnY, currentAngle); - // _swerveDriveTable->GetEntry("RotateMatch") - // .SetDouble(_requestedAngle.value()); - // _swerveDrivebase->RotateMatchJoystick( - // _requestedAngle, - // wom::drivetrain::FieldRelativeSpeeds{// also field relative - // xVelocity * maxMovementMagnitude, - // yVelocity * maxMovementMagnitude, - // r_x * maxRotationMagnitude}); - // } else { - _swerveDrivebase->SetFieldRelativeVelocity( - wom::drivetrain::FieldRelativeSpeeds{xVelocity * -maxMovementMagnitude, - yVelocity * -maxMovementMagnitude, - r_x * -maxRotationMagnitude}); - - // _swerveDrivebase->SetVelocity( - // frc::ChassisSpeeds{xVelocity * maxMovementMagnitude, - // yVelocity * maxMovementMagnitude, - // r_x * maxRotationMagnitude}); + // _swerveDrivebase->SetVelocity( + // frc::ChassisSpeeds{xVelocity * maxMovementMagnitude, + // yVelocity * maxMovementMagnitude, + // r_x * maxRotationMagnitude}); // } // } // _swerveDrivebase->SetIndividualTuning(2, 0_deg, 0_mps); @@ -121,8 +120,7 @@ void ManualDrivebase::ResetMode() { resetMode = false; } -void ManualDrivebase::CalculateRequestedAngle(double joystickX, - double joystickY, +void ManualDrivebase::CalculateRequestedAngle(double joystickX, double joystickY, units::degree_t defaultAngle) { _requestedAngle = (1_rad * std::atan2(joystickY, -joystickX)) + 90_deg; if (wom::utils::deadzone(joystickX) == 0 && wom::utils::deadzone(joystickY) == 0) { @@ -131,8 +129,7 @@ void ManualDrivebase::CalculateRequestedAngle(double joystickX, } // Code for x-ing the wheels on the drivebase -XDrivebase::XDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase) - : _swerveDrivebase(swerveDrivebase) { +XDrivebase::XDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase) : _swerveDrivebase(swerveDrivebase) { Controls(swerveDrivebase); } void XDrivebase::OnTick(units::second_t deltaTime) { @@ -174,70 +171,134 @@ void XDrivebase::OnTick(units::second_t deltaTime) { // m_timer.Start(); // } -wom::drivetrain::behaviours::TempSimSwerveDrive::TempSimSwerveDrive( - frc::Timer* timer, frc::Field2d* field) - : m_timer(timer), m_field(field) {} +// wom::drivetrain::behaviours::TempSimSwerveDrive::TempSimSwerveDrive(frc::Timer* timer, frc::Field2d* field) +// : m_timer(timer), m_field(field) {} +// +// void wom::drivetrain::behaviours::TempSimSwerveDrive::OnUpdate() { +// m_field->SetRobotPose(m_driveSim.GetPose()); +// +// // get the current trajectory state +// frc::Trajectory::State desired_state = current_trajectory.Sample(m_timer->Get()); +// +// // get the current wheel speeds +// wom::utils::WriteTrajectoryState(current_trajectory_state_table, desired_state); +// +// // move drivebase position to the desired state +// m_driveSim.SetPose(wom::utils::TrajectoryStateToPose2d(desired_state)); +// +// // update the drivebase +// m_driveSim.Update(20_ms); +// } +// +// frc::Pose3d wom::drivetrain::behaviours::TempSimSwerveDrive::GetPose() { +// frc::Pose3d currentPose{m_driveSim.GetPose()}; +// return currentPose; +// } +// +// frc::Pose2d wom::drivetrain::behaviours::TempSimSwerveDrive::GetPose2d() { +// return m_driveSim.GetPose(); +// } +// +// void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(std::string path) { +// nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault(); +// std::shared_ptr table = inst.GetTable("FMSInfo"); +// +// // create a netowrk table for the trajectory +// std::shared_ptr trajectory_table = +// nt::NetworkTableInstance::GetDefault().GetTable("trajectory_path"); +// current_trajectory_table = nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory"); +// current_trajectory_state_table = +// nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory_state"); +// +// current_trajectory = m_pathplanner.getTrajectory(path); +// m_driveSim.SetPose(current_trajectory.Sample(0_s).pose); +// m_timer->Reset(); +// m_timer->Start(); +// } +// +// wom::drivetrain::behaviours::AutoSwerveDrive::AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve, +// frc::Timer* timer, frc::Field2d* field) +// : _swerve(swerve), m_timer(timer), m_field(field) { +// _simSwerveDrive = new wom::drivetrain::behaviours::TempSimSwerveDrive(timer, field); +// } +// +// void wom::drivetrain::behaviours::AutoSwerveDrive::OnUpdate() { +// _simSwerveDrive->OnUpdate(); +// _swerve->SetPose(_simSwerveDrive->GetPose2d()); +// } +// +// void wom::drivetrain::behaviours::AutoSwerveDrive::SetPath(std::string path) { +// _simSwerveDrive->SetPath(path); +// } + +wom::drivetrain::behaviours::DrivebasePoseBehaviour::DrivebasePoseBehaviour(SwerveDrive* swerveDrivebase, + frc::Pose2d pose, + units::volt_t voltageLimit, + bool hold) + : _swerveDrivebase(swerveDrivebase), _pose(pose), _hold(hold), _voltageLimit(voltageLimit) { + Controls(swerveDrivebase); +} -void wom::drivetrain::behaviours::TempSimSwerveDrive::OnUpdate() { - m_field->SetRobotPose(m_driveSim.GetPose()); +void wom::drivetrain::behaviours::DrivebasePoseBehaviour::OnStart() { + if (_voltageLimit >= (frc::RobotController::GetBatteryVoltage() - 0.5_V)) { + _voltageLimit = frc::RobotController::GetBatteryVoltage() - 1_V; + } - // get the current trajectory state - frc::Trajectory::State desired_state = - current_trajectory.Sample(m_timer->Get()); + double currentAngle = _swerveDrivebase->GetPose().Rotation().Radians().value(); - // get the current wheel speeds - wom::utils::WriteTrajectoryState(current_trajectory_state_table, - desired_state); + units::radian_t adjustedAngle = + 1_rad * (currentAngle - std::fmod(currentAngle, 360) + _pose.Rotation().Radians().value() - 3.14159); - // move drivebase position to the desired state - m_driveSim.SetPose(wom::utils::TrajectoryStateToPose2d(desired_state)); + _swerveDrivebase->SetVoltageLimit(_voltageLimit); - // update the drivebase - m_driveSim.Update(20_ms); -} + // _swerveDrivebase->SetPose(frc::Pose2d{_pose.X(), _pose.Y(), adjustedAngle}); + _swerveDrivebase->TurnToAngle(adjustedAngle); -frc::Pose3d wom::drivetrain::behaviours::TempSimSwerveDrive::GetPose() { - frc::Pose3d currentPose{m_driveSim.GetPose()}; - return currentPose; + _timer.Start(); } -frc::Pose2d wom::drivetrain::behaviours::TempSimSwerveDrive::GetPose2d() { - return m_driveSim.GetPose(); +// used in autonomous for going to set drive poses +void wom::drivetrain::behaviours::DrivebasePoseBehaviour::OnTick(units::second_t deltaTime) { +#ifdef DEBUG + nt::NetworkTableInstance::GetDefault().GetTable("drivetrainpose")->GetEntry("going").SetBoolean(true); +#endif + if (_timer.Get() > 1_s) { +#ifdef DEBUG + nt::NetworkTableInstance::GetDefault().GetTable("drivetrainpose")->GetEntry("going").SetBoolean(false); +#endif + SetDone(); + } + if (_swerveDrivebase->IsAtSetAngle() && _swerveDrivebase->GetState() == SwerveDriveState::kAngle) { + _swerveDrivebase->SetPose(frc::Pose2d(_pose.X(), _pose.Y(), 0_deg)); + } else { + if (_swerveDrivebase->IsAtSetPose() && !_hold) { + #ifdef DEBUG + std::cout << "Exited..." << std::endl; + nt::NetworkTableInstance::GetDefault().GetTable("drivetrainpose")->GetEntry("going").SetBoolean(false); + #endif + SetDone(); + } + } } -void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath( - std::string path) { - nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault(); - std::shared_ptr table = inst.GetTable("FMSInfo"); - - // create a netowrk table for the trajectory - std::shared_ptr trajectory_table = - nt::NetworkTableInstance::GetDefault().GetTable("trajectory_path"); - current_trajectory_table = - nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory"); - current_trajectory_state_table = - nt::NetworkTableInstance::GetDefault().GetTable( - "current_trajectory_state"); - - current_trajectory = m_pathplanner.getTrajectory(path); - m_driveSim.SetPose(current_trajectory.Sample(0_s).pose); - m_timer->Reset(); - m_timer->Start(); +wom::drivetrain::behaviours::TurnToAngleBehaviour::TurnToAngleBehaviour(wom::drivetrain::SwerveDrive* swerve, units::radian_t angle) : _angle(angle), _swerve(swerve) { + Controls(swerve); } -wom::drivetrain::behaviours::AutoSwerveDrive::AutoSwerveDrive( - wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer, - frc::Field2d* field) - : _swerve(swerve), m_timer(timer), m_field(field) { - _simSwerveDrive = - new wom::drivetrain::behaviours::TempSimSwerveDrive(timer, field); -} +void wom::drivetrain::behaviours::TurnToAngleBehaviour::OnTick(units::second_t dt) { + _swerve->TurnToAngle(_angle); -void wom::drivetrain::behaviours::AutoSwerveDrive::OnUpdate() { - _simSwerveDrive->OnUpdate(); - _swerve->SetPose(_simSwerveDrive->GetPose2d()); -} +#ifdef DEBUG + nt::NetworkTableInstance::GetDefault().GetTable("drivetrain")->GetEntry("targetAngle").SetDouble(_angle.value()); + nt::NetworkTableInstance::GetDefault().GetTable("drivetrain")->GetEntry("runningangle").SetBoolean(true); +#endif -void wom::drivetrain::behaviours::AutoSwerveDrive::SetPath(std::string path) { - _simSwerveDrive->SetPath(path); + if (units::math::abs(_swerve->GetPose().Rotation().Radians() - _angle) < 0.1_rad) { +#ifdef DEBUG + nt::NetworkTableInstance::GetDefault().GetTable("drivetrain")->GetEntry("runningangle").SetBoolean(false); +#endif + SetDone(); + } } + + diff --git a/wombat/src/main/cpp/sim/Sim.cpp b/wombat/src/main/cpp/sim/Sim.cpp new file mode 100644 index 00000000..914394de --- /dev/null +++ b/wombat/src/main/cpp/sim/Sim.cpp @@ -0,0 +1,25 @@ +// Copyright (c) 2023-2024 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + +#include "sim/Sim.h" + +#include "drivetrain/SwerveDrive.h" +#include "frc/geometry/Pose2d.h" +#include "frc/smartdashboard/SmartDashboard.h" +#include "networktables/NetworkTableInstance.h" +#include "utils/Util.h" + +wom::sim::SimSwerve::SimSwerve(wom::drivetrain::SwerveDrive* _swerve) : _swerve(_swerve) { + frc::SmartDashboard::PutData("Field", &_field); +} + +void wom::sim::SimSwerve::OnTick() { + _field.SetRobotPose(_swerve->GetPose()); + utils::WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("swerveSetpoint"), + _swerve->GetSetpoint()); +} + +void wom::sim::SimSwerve::OnTick(frc::Pose2d pose) { + _field.SetRobotPose(pose); +} diff --git a/wombat/src/main/cpp/subsystems/Arm.cpp b/wombat/src/main/cpp/subsystems/Arm.cpp index 9390601a..a451d5cf 100644 --- a/wombat/src/main/cpp/subsystems/Arm.cpp +++ b/wombat/src/main/cpp/subsystems/Arm.cpp @@ -10,19 +10,14 @@ using namespace frc; using namespace wom; // creates network table instatnce on shuffleboard -void wom::subsystems::ArmConfig::WriteNT( - std::shared_ptr table) { +void wom::subsystems::ArmConfig::WriteNT(std::shared_ptr table) { table->GetEntry("armMass").SetDouble(armMass.value()); table->GetEntry("loadMass").SetDouble(loadMass.value()); table->GetEntry("armLength").SetDouble(armLength.value()); - table->GetEntry("minAngle") - .SetDouble(minAngle.convert().value()); - table->GetEntry("maxAngle") - .SetDouble(maxAngle.convert().value()); - table->GetEntry("initialAngle") - .SetDouble(initialAngle.convert().value()); - table->GetEntry("angleOffset") - .SetDouble(initialAngle.convert().value()); + table->GetEntry("minAngle").SetDouble(minAngle.convert().value()); + table->GetEntry("maxAngle").SetDouble(maxAngle.convert().value()); + table->GetEntry("initialAngle").SetDouble(initialAngle.convert().value()); + table->GetEntry("angleOffset").SetDouble(initialAngle.convert().value()); } // arm config is used @@ -43,14 +38,12 @@ void wom::subsystems::Arm::OnUpdate(units::second_t dt) { case ArmState::kIdle: break; case ArmState::kVelocity: { - units::newton_meter_t torque = - 9.81_m / 1_s / 1_s * _config.armLength * - units::math::cos(angle + _config.angleOffset) * - (0.5 * _config.armMass + _config.loadMass); + units::newton_meter_t torque = 9.81_m / 1_s / 1_s * _config.armLength * + units::math::cos(angle + _config.angleOffset) * + (0.5 * _config.armMass + _config.loadMass); // units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, // 0_rad/1_s); - units::volt_t feedforward = - _config.leftGearbox.motor.Voltage(torque, _velocityPID.GetSetpoint()); + units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, _velocityPID.GetSetpoint()); // feedforward = 3.5_V; // std::cout << "feedforward" << feedforward.value() << std::endl; voltage = _velocityPID.Calculate(GetArmVelocity(), dt, feedforward); @@ -58,12 +51,10 @@ void wom::subsystems::Arm::OnUpdate(units::second_t dt) { // std::endl; voltage = 0_V; } break; case ArmState::kAngle: { - units::newton_meter_t torque = - 9.81_m / 1_s / 1_s * _config.armLength * - units::math::cos(angle + _config.angleOffset) * - (0.5 * _config.armMass + _config.loadMass); - units::volt_t feedforward = - _config.leftGearbox.motor.Voltage(torque, 0_rad / 1_s); + units::newton_meter_t torque = 9.81_m / 1_s / 1_s * _config.armLength * + units::math::cos(angle + _config.angleOffset) * + (0.5 * _config.armMass + _config.loadMass); + units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, 0_rad / 1_s); // std::cout << "feedforward" << feedforward.value() << std::endl; voltage = _pid.Calculate(angle, dt, feedforward); } break; @@ -123,8 +114,12 @@ wom::subsystems::ArmConfig& wom::subsystems::Arm::GetConfig() { return _config; } +wom::subsystems::ArmState wom::subsystems::Arm::GetState() { + return _state; +} + units::radian_t wom::subsystems::Arm::GetAngle() const { - return _config.armEncoder.GetPosition() / 100 * 360 * 1_deg; + return _config.armEncoder->GetPosition() / 100 * 360 * 1_deg; } units::radians_per_second_t wom::subsystems::Arm::MaxSpeed() const { @@ -132,7 +127,7 @@ units::radians_per_second_t wom::subsystems::Arm::MaxSpeed() const { } units::radians_per_second_t wom::subsystems::Arm::GetArmVelocity() const { - return _config.armEncoder.GetVelocity() / 100 * 360 * 1_deg / 60_s; + return _config.armEncoder->GetVelocity() / 100 * 360 * 1_deg / 60_s; } bool wom::subsystems::Arm::IsStable() const { diff --git a/wombat/src/main/cpp/subsystems/Elevator.cpp b/wombat/src/main/cpp/subsystems/Elevator.cpp index d6b59f8a..d1cf9a8f 100644 --- a/wombat/src/main/cpp/subsystems/Elevator.cpp +++ b/wombat/src/main/cpp/subsystems/Elevator.cpp @@ -7,8 +7,7 @@ #include #include -void wom::subsystems::ElevatorConfig::WriteNT( - std::shared_ptr table) { +void wom::subsystems::ElevatorConfig::WriteNT(std::shared_ptr table) { table->GetEntry("radius").SetDouble(radius.value()); table->GetEntry("mass").SetDouble(mass.value()); table->GetEntry("maxHeight").SetDouble(maxHeight.value()); @@ -20,8 +19,7 @@ wom::subsystems::Elevator::Elevator(ElevatorConfig config) _pid{config.path + "/pid", config.pid}, _velocityPID{config.path + "/velocityPID", config.velocityPID}, _table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)) { - _config.leftGearbox.encoder->SetEncoderPosition(_config.initialHeight / - _config.radius * 1_rad); + _config.leftGearbox.encoder->SetEncoderPosition(_config.initialHeight / _config.radius * 1_rad); } void wom::subsystems::Elevator::OnUpdate(units::second_t dt) { @@ -39,8 +37,7 @@ void wom::subsystems::Elevator::OnUpdate(units::second_t dt) { case ElevatorState::kVelocity: { units::volt_t feedforward = _config.rightGearbox.motor.Voltage( (_config.mass * 9.81_mps_sq) * _config.radius, - _velocityPID.GetSetpoint() / - (14.0 / 60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) * 1_rad); + _velocityPID.GetSetpoint() / (14.0 / 60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) * 1_rad); // units::volt_t feedforward = _config.rightGearbox.motor.Voltage(0_Nm, // _velocityPID.GetSetpoint() / (14.0/60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) // * 1_rad); @@ -53,8 +50,8 @@ void wom::subsystems::Elevator::OnUpdate(units::second_t dt) { // voltage = 0_V; } break; case ElevatorState::kPID: { - units::volt_t feedforward = _config.rightGearbox.motor.Voltage( - (_config.mass * 9.81_mps_sq) * _config.radius, 0_rad_per_s); + units::volt_t feedforward = + _config.rightGearbox.motor.Voltage((_config.mass * 9.81_mps_sq) * _config.radius, 0_rad_per_s); // std::cout << "feed forward" << feedforward.value() << std::endl; feedforward = 1.2_V; // voltage = _pid.Calculate(height, dt, feedforward); @@ -99,8 +96,7 @@ void wom::subsystems::Elevator::SetPID(units::meter_t height) { _pid.SetSetpoint(height); } -void wom::subsystems::Elevator::SetVelocity( - units::meters_per_second_t velocity) { +void wom::subsystems::Elevator::SetVelocity(units::meters_per_second_t velocity) { _velocityPID.SetSetpoint(velocity); } @@ -132,18 +128,14 @@ units::meter_t wom::subsystems::Elevator::GetHeight() const { // std::cout << "elevator position"<< // _config.rightGearbox.encoder->GetEncoderTicks() << std::endl; return // _config.rightGearbox.encoder->GetEncoderDistance() * 1_m; - return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 * - 0.02225 * 1_m; + return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 * 0.02225 * 1_m; } -units::meters_per_second_t wom::subsystems::Elevator::GetElevatorVelocity() - const { - return _config.elevatorEncoder.GetVelocity() / 60_s * 14 / 60 * 2 * 3.1415 * - 0.02225 * 1_m; +units::meters_per_second_t wom::subsystems::Elevator::GetElevatorVelocity() const { + return _config.elevatorEncoder.GetVelocity() / 60_s * 14 / 60 * 2 * 3.1415 * 0.02225 * 1_m; } units::meters_per_second_t wom::subsystems::Elevator::MaxSpeed() const { - return _config.leftGearbox.motor.Speed( - (_config.mass * 9.81_mps_sq) * _config.radius, 12_V) / - 1_rad * _config.radius; + return _config.leftGearbox.motor.Speed((_config.mass * 9.81_mps_sq) * _config.radius, 12_V) / 1_rad * + _config.radius; } diff --git a/wombat/src/main/cpp/subsystems/Shooter.cpp b/wombat/src/main/cpp/subsystems/Shooter.cpp index 2fede2f3..209134d1 100644 --- a/wombat/src/main/cpp/subsystems/Shooter.cpp +++ b/wombat/src/main/cpp/subsystems/Shooter.cpp @@ -14,16 +14,14 @@ wom::subsystems::Shooter::Shooter(std::string path, ShooterParams params) void wom::subsystems::Shooter::OnUpdate(units::second_t dt) { units::volt_t voltage{0}; - units::revolutions_per_minute_t currentSpeed = - _params.gearbox.encoder->GetEncoderAngularVelocity(); + units::revolutions_per_minute_t currentSpeed = _params.gearbox.encoder->GetEncoderAngularVelocity(); switch (_state) { case ShooterState::kManual: voltage = _setpointManual; break; case ShooterState::kPID: { - auto feedforward = - _params.gearbox.motor.Voltage(0_Nm, _pid.GetSetpoint()); + auto feedforward = _params.gearbox.motor.Voltage(0_Nm, _pid.GetSetpoint()); voltage = _pid.Calculate(currentSpeed, dt, feedforward); } break; case ShooterState::kIdle: @@ -31,21 +29,20 @@ void wom::subsystems::Shooter::OnUpdate(units::second_t dt) { break; } - units::newton_meter_t max_torque_at_current_limit = - _params.gearbox.motor.Torque(_params.currentLimit); + units::newton_meter_t max_torque_at_current_limit = _params.gearbox.motor.Torque(_params.currentLimit); units::volt_t max_voltage_for_current_limit = _params.gearbox.motor.Voltage(max_torque_at_current_limit, currentSpeed); - voltage = - 1_V * std::min(voltage.value(), max_voltage_for_current_limit.value()); + voltage = 1_V * std::min(voltage.value(), max_voltage_for_current_limit.value()); // _params.gearbox.motorController->SetVoltage(voltage); + #ifdef DEBUG _table->GetEntry("output_volts").SetDouble(voltage.value()); _table->GetEntry("speed_rpm").SetDouble(currentSpeed.value()); - _table->GetEntry("setpoint_rpm") - .SetDouble(units::revolutions_per_minute_t{_pid.GetSetpoint()}.value()); + _table->GetEntry("setpoint_rpm").SetDouble(units::revolutions_per_minute_t{_pid.GetSetpoint()}.value()); _table->GetEntry("stable").SetBoolean(_pid.IsStable()); + #endif } void wom::subsystems::Shooter::SetManual(units::volt_t voltage) { @@ -67,8 +64,7 @@ bool wom::subsystems::Shooter::IsStable() const { // Shooter Manual Set -wom::subsystems::ShooterConstant::ShooterConstant(Shooter* s, - units::volt_t setpoint) +wom::subsystems::ShooterConstant::ShooterConstant(Shooter* s, units::volt_t setpoint) : _shooter(s), _setpoint(setpoint) { Controls(_shooter); } @@ -79,9 +75,7 @@ void wom::subsystems::ShooterConstant::OnTick(units::second_t dt) { // ShooterSpinup -wom::subsystems::ShooterSpinup::ShooterSpinup(Shooter* s, - units::radians_per_second_t speed, - bool hold) +wom::subsystems::ShooterSpinup::ShooterSpinup(Shooter* s, units::radians_per_second_t speed, bool hold) : _shooter(s), _speed(speed), _hold(hold) { Controls(_shooter); } diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp index 744c0590..8f3b2c87 100644 --- a/wombat/src/main/cpp/utils/Encoder.cpp +++ b/wombat/src/main/cpp/utils/Encoder.cpp @@ -6,8 +6,10 @@ #include -wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type, - units::meter_t wheelRadius, double reduction) +#include + +wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius, + double reduction) : _reduction(reduction), _encoderTicksPerRotation(encoderTicksPerRotation), _type(type), @@ -32,8 +34,8 @@ void wom::utils::Encoder::SetEncoderPosition(units::degree_t position) { // _offset = -offset_turns; } -void wom::utils::Encoder::SetEncoderOffset(units::radian_t offset) { //HERE! - _offset = offset; +void wom::utils::Encoder::SetEncoderOffset(units::radian_t offset) { // HERE! + _offset = offset; // units::turn_t offset_turns = offset; // _offset = offset_turns.value() * GetEncoderTicksPerRotation(); } @@ -43,16 +45,16 @@ void wom::utils::Encoder::SetReduction(double reduction) { } units::radian_t wom::utils::Encoder::GetEncoderPosition() { - //if (_type == 0) { - // units::turn_t n_turns{GetEncoderTicks() / GetEncoderTicksPerRotation()}; - // return n_turns; - //} else if (_type == 2) { - // units::degree_t pos = GetEncoderTicks() * 1_deg; - // return pos; - //} else { - // units::degree_t pos = GetEncoderTicks() * 1_deg; - // return pos - _offset; - //} + // if (_type == 0) { + // units::turn_t n_turns{GetEncoderTicks() / GetEncoderTicksPerRotation()}; + // return n_turns; + // } else if (_type == 2) { + // units::degree_t pos = GetEncoderTicks() * 1_deg; + // return pos; + // } else { + // units::degree_t pos = GetEncoderTicks() * 1_deg; + // return pos - _offset; + // } return GetEncoderTicks() * 1_rad; } @@ -63,8 +65,7 @@ double wom::utils::Encoder::GetEncoderDistance() { units::radians_per_second_t wom::utils::Encoder::GetEncoderAngularVelocity() { // return GetEncoderTickVelocity() / (double)GetEncoderTicksPerRotation() * 2 // * 3.1415926; - units::turns_per_second_t n_turns_per_s{GetEncoderTickVelocity() / - GetEncoderTicksPerRotation()}; + units::turns_per_second_t n_turns_per_s{GetEncoderTickVelocity() / GetEncoderTicksPerRotation()}; return n_turns_per_s; } @@ -86,15 +87,12 @@ double wom::utils::DigitalEncoder::GetEncoderTickVelocity() const { return _nativeEncoder.GetRate(); } -wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller, - units::meter_t wheelRadius, +wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller, units::meter_t wheelRadius, double reduction) : wom::utils::Encoder(42, reduction, wheelRadius, 2), _encoder(controller->GetEncoder(rev::SparkRelativeEncoder::Type::kHallSensor)) {} - - double wom::utils::CANSparkMaxEncoder::GetEncoderRawTicks() const { - return _encoder.GetPosition() * _reduction; + return ((_encoder.GetPosition() * 2 * 3.1415) / 200); } double wom::utils::CANSparkMaxEncoder::GetEncoderTickVelocity() const { @@ -121,11 +119,9 @@ double wom::utils::CANSparkMaxEncoder::GetVelocity() const { return _encoder.GetVelocity(); } -wom::utils::TalonFXEncoder::TalonFXEncoder( - ctre::phoenix6::hardware::TalonFX* controller, units::meter_t wheelRadius, - double reduction) - : utils::Encoder(2048, reduction, wheelRadius, 0), - _controller(controller) {} +wom::utils::TalonFXEncoder::TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller, + units::meter_t wheelRadius, double reduction) + : utils::Encoder(2048, reduction, wheelRadius, 0), _controller(controller) {} double wom::utils::TalonFXEncoder::GetEncoderRawTicks() const { return _controller->GetPosition().GetValue().value(); @@ -138,9 +134,8 @@ double wom::utils::TalonFXEncoder::GetEncoderTickVelocity() const { wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel, units::meter_t wheelRadius, double ticksPerRotation, double reduction) : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 0) { - _dutyCycleEncoder = new frc::DutyCycleEncoder(channel); - - } + _dutyCycleEncoder = new frc::DutyCycleEncoder(channel); +} double wom::utils::DutyCycleEncoder::GetEncoderRawTicks() const { return _dutyCycleEncoder->GetAbsolutePosition(); @@ -149,11 +144,12 @@ double wom::utils::DutyCycleEncoder::GetEncoderRawTicks() const { double wom::utils::DutyCycleEncoder::GetEncoderTickVelocity() const { return 0; } -wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius, - double ticksPerRotation, double reduction, - std::string name) + +wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius, double ticksPerRotation, + double reduction, std::string name) : wom::utils::Encoder(ticksPerRotation, 2, wheelRadius, reduction) { _canEncoder = new ctre::phoenix6::hardware::CANcoder(deviceNumber, name); + // _canEncoder->ConfigAbsoluteEncoderRange(0, 1); } double wom::utils::CanEncoder::GetEncoderRawTicks() const { diff --git a/wombat/src/main/cpp/utils/Pathplanner.cpp b/wombat/src/main/cpp/utils/Pathplanner.cpp index cc4752ae..46bc4032 100644 --- a/wombat/src/main/cpp/utils/Pathplanner.cpp +++ b/wombat/src/main/cpp/utils/Pathplanner.cpp @@ -4,15 +4,716 @@ #include "utils/Pathplanner.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "behaviour/Behaviour.h" +#include "drivetrain/SwerveDrive.h" +#include "drivetrain/behaviours/SwerveBehaviours.h" +#include "frc/Filesystem.h" +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/geometry/Transform2d.h" +#include "frc/kinematics/ChassisSpeeds.h" +#include "frc/smartdashboard/SmartDashboard.h" +#include "networktables/NetworkTable.h" +#include "networktables/NetworkTableInstance.h" +#include "pathplanner/lib/path/PathPlannerPath.h" +#include "pathplanner/lib/path/PathPoint.h" +#include "pathplanner/lib/path/RotationTarget.h" +#include "units/acceleration.h" +#include "units/base.h" +#include "units/time.h" +#include "utils/Util.h" +#include "wpi/detail/conversions/to_json.h" +#include "wpi/json_fwd.h" + using namespace wom; -utils::Pathplanner::Pathplanner() {} +wom::utils::BezierPoint::BezierPoint(double anchorX, double anchorY, wpi::json prevControlX, + wpi::json prevControlY, wpi::json nextControlX, wpi::json nextControlY, + bool isLocked, wpi::json linkedName) + : anchor({anchorX, anchorY}), + prevControl{prevControlX.is_null() ? NAN : prevControlX.get(), + prevControlY.is_null() ? NAN : prevControlY.get()}, + nextControl{nextControlX.is_null() ? NAN : nextControlX.get(), + nextControlY.is_null() ? NAN : nextControlY.get()}, + isLocked(isLocked), + linkedName(linkedName.is_null() ? "" : linkedName.get()) {} + +double wom::utils::BezierPoint::calculateAngle() const { + return std::atan2(anchor.y - prevControl.y, anchor.x - prevControl.x); +} + +double wom::utils::distance(const wom::utils::BezierPoint& p1, const wom::utils::BezierPoint& p2) { + return std::sqrt(std::pow(p2.anchor.x - p1.anchor.x, 2) + std::pow(p2.anchor.y - p1.anchor.y, 2)); +} + +std::vector wom::utils::interpolateAtIntervals(const wom::utils::BezierPoint& p1, + const wom::utils::BezierPoint& p2, + double interval) { + std::vector interpolatedPoints; + + double totalDistance = distance(p1, p2); + int numPoints = static_cast(std::ceil(totalDistance / interval)); + + for (int i = 1; i <= numPoints; ++i) { + double t = static_cast(i) / (numPoints + 1); + interpolatedPoints.emplace_back( + p1.anchor.x + t * (p2.anchor.x - p1.anchor.x), p1.anchor.y + t * (p2.anchor.y - p1.anchor.y), + p1.prevControl.x + t * (p2.prevControl.x - p1.prevControl.x), + p1.prevControl.y + t * (p2.prevControl.y - p1.prevControl.y), + p1.nextControl.x + t * (p2.nextControl.x - p1.nextControl.x), + p1.nextControl.y + t * (p2.nextControl.y - p1.nextControl.y), p1.isLocked, p1.linkedName); + } + + return interpolatedPoints; +} + +std::vector wom::utils::createBezierPointsFromJson(const wpi::json& jsonData) { + std::vector bezierPoints; + + for (const auto& point : jsonData["waypoints"]) { + auto anchorX = point["anchor"].is_null() ? NAN : point["anchor"]["x"].get(); + auto anchorY = point["anchor"].is_null() ? NAN : point["anchor"]["y"].get(); + auto prevControlX = point["prevControl"].is_null() ? NAN : point["prevControl"]["x"].get(); + auto prevControlY = point["prevControl"].is_null() ? NAN : point["prevControl"]["y"].get(); + auto nextControlX = point["nextControl"].is_null() ? NAN : point["nextControl"]["x"].get(); + auto nextControlY = point["nextControl"].is_null() ? NAN : point["nextControl"]["y"].get(); + auto isLocked = point["isLocked"].is_null() ? false : point["isLocked"].get(); + auto linkedName = point["linkedName"].is_null() ? "" : point["linkedName"].get(); + + bezierPoints.emplace_back(anchorX, anchorY, prevControlX, prevControlY, nextControlX, nextControlY, + isLocked, linkedName); + } + + return bezierPoints; +} + +std::vector wom::utils::interpolateBezierPoints( + const std::vector& bezierPoints, double interval) { + std::vector interpolatedPoints; -frc::Trajectory utils::Pathplanner::getTrajectory(std::string_view path) { + for (size_t i = 0; i < bezierPoints.size() - 1; ++i) { + auto points = interpolateAtIntervals(bezierPoints[i], bezierPoints[i + 1], interval); + interpolatedPoints.insert(interpolatedPoints.end(), points.begin(), points.end()); + } + + return interpolatedPoints; +} + +// Command implementation +// template +// utils::Command::Command(std::string name, FunctionType func) +// : name_(std::move(name)), function_(std::move(func)), argumentTypes_{typeid(Args)...} {} + +// template +// Ret utils::Command::Execute(Args... args) const { +// return function_(std::forward(args)...); +// } + +// template +// std::string utils::Command::GetName() const { +// return name_; +// } + +// template +// bool utils::Command::IsValid(std::vector argTypes) const { +// return argumentTypes_ == argTypes; +// } + +// Commands implementation +// template +// template +// utils::Commands::Commands(CommandArgs&&... commands) +// : +// commands_{std::make_shared>(std::forward(commands))...} +// {} +// +// template +// template +// Ret utils::Commands::Execute(std::string command, Args&&... args) { +// auto it = std::find_if(commands_.begin(), commands_.end(), [&command](const auto& cmd) { +// return cmd->GetName() == command; +// }); +// +// if (it != commands_.end()) { +// return (*it)->template Execute({std::forward(args)...}); +// } else { +// throw std::invalid_argument("Command not found"); +// } +// } +// +// template +// bool utils::Commands::IsValid(std::string command, std::vector argTypes) +// const { +// auto it = std::find_if(commands_.begin(), commands_.end(), [&command](const auto& cmd) { +// return cmd->GetName() == command; +// }); +// +// if (it != commands_.end()) { +// return (*it)->IsValid(argTypes); +// } else { +// throw std::invalid_argument("Command not found"); +// } +// } +// +// template +// template +// Ret utils::Commands::Run(std::string commandString, Args&&... args) { +// size_t pos = commandString.find("("); +// if (pos != std::string::npos) { +// std::string command = commandString.substr(0, pos); +// std::string argString = commandString.substr(pos + 1, commandString.size() - pos - 2); +// +// std::vector parsedArgs = ParseArguments(argString); +// +// if (IsValid(command, GetTypeIndices())) { +// return Execute(command, std::forward(args)...); +// } else { +// throw std::invalid_argument("Invalid command or argument types"); +// } +// } else { +// throw std::invalid_argument("Invalid command string format"); +// } +// } +// +// template +// std::vector utils::Commands::ParseArguments(const std::string& argString) { +// std::vector args; +// +// std::istringstream iss(argString); +// std::string arg; +// while (iss >> arg) { +// args.push_back(ParseSingleArgument(arg)); +// } +// +// return args; +// } +// +// template +// std::any utils::Commands::ParseSingleArgument(const std::string& arg) { +// try { +// return std::stoi(arg); +// } catch (const std::invalid_argument& e) { +// throw std::invalid_argument("Error parsing argument: " + arg); +// } +// } +// +// template +// template +// std::type_index utils::Commands::GetTypeIndex() { +// return std::type_index(typeid(T)); +// } + +// PathWeaver implementation +utils::PathWeaver::PathWeaver() {} + +frc::Trajectory utils::PathWeaver::getTrajectory(std::string_view path) { try { fs::path path_location = deploy_directory / path; return frc::TrajectoryUtil::FromPathweaverJson(path_location.string()); - } catch (std::exception& e) { - return getTrajectory(path); + } catch (const std::exception& e) { + // Handle the exception appropriately or consider removing the recursive call + throw; // Rethrow the exception for now } } + +// FollowPath implementation +utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path, bool flip, frc::Pose2d offset) + : _swerve(swerve), behaviour::Behaviour(""), _offset(offset), _pathName(path) { + Controls(swerve); + + _path = pathplanner::PathPlannerPath::fromPathFile(path); + + if (flip) { + _path = _path->flipPath(); + } + + std::string filePath = frc::filesystem::GetDeployDirectory() + "/pathplanner/paths/" + path + ".path"; + + #ifdef DEBUG + std::cout << filePath << std::endl; + #endif + + std::ifstream file(filePath); + + #ifdef DEBUG + nt::NetworkTableInstance::GetDefault() + .GetTable("pathplanner") + ->GetEntry("Current path") + .SetString(filePath); + #endif + + std::string cdata; + std::string cjson; + + while (file.good()) { + file >> cdata; + cjson += cdata; + } + + // cjson.pop_back(); + + std::vector points = + pathplanner::PathPlannerPath::fromPathFile(path)->getAllPathPoints(); + + points = CheckPoints(points); + + pathplanner::RotationTarget* lastRot = nullptr; + units::degree_t rot; + + int amt = 1; + size_t tot = points.size(); + int index = std::round((static_cast(amt) / static_cast(tot)) * 100); + // double index = (amt / tot) * 100; + int i = 0; + bool f = true; + + nt::NetworkTableInstance::GetDefault().GetTable("pathplanner")->GetEntry("index").SetDouble(index); + nt::NetworkTableInstance::GetDefault().GetTable("pathplanner")->GetEntry("total").SetInteger(tot); + + for (const pathplanner::PathPoint& point : points) { + if (lastRot != nullptr) { + rot = point.rotationTarget.value_or(*lastRot).getTarget().Degrees(); + } else { + rot = point.rotationTarget->getTarget().Degrees(); + } + + pathplanner::RotationTarget t = pathplanner::RotationTarget(0, rot, false); + + lastRot = &t; + + frc::Translation2d tr = frc::Translation2d(point.position.X() * -1, point.position.Y() * -1); + frc::Pose2d pose2 = frc::Pose2d(tr, rot); //.TransformBy(frc::Transform2d(-1.37_m, -5.56_m, 0_deg)); + + if (i == index || i == static_cast(tot - 1) || f) { + WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("pathplanner/offset"), _offset); // -6 -6 + _poses.emplace_back(frc::Pose2d(pose2.X() - _offset.X(), pose2.Y() - _offset.Y(), 0_deg)); + // _poses.emplace_back(frc::Pose2d(pose2.Y(), pose2.X(), pose2.Rotation())); + i = 0; // - 5.56_m, - 2.91_m + f = false; + } + + i++; + } + // _poses = pathplanner::PathPlannerPath::fromPathFile(path)->getPathPoses(); + // wpi::json j = wpi::json::parse(cjson); + // + // std::vector bezierPoints = createBezierPointsFromJson(j); + // + // std::vector interpolatedPoints = interpolateBezierPoints(bezierPoints, 0.2); + // + // int i = 0; + // for (const BezierPoint& point : interpolatedPoints) { + // frc::Rotation2d a; + // + // if (point.calculateAngle() != NAN) { + // a = frc::Rotation2d(units::degree_t{point.calculateAngle()}); + // } else { + // a = _swerve->GetPose().Rotation(); + // } + // + // frc::Pose2d p = frc::Pose2d(frc::Translation2d(units::meter_t{point.anchor.x}, + // units::meter_t{point.anchor.y}), a); frc::Pose2d p1 = + // frc::Pose2d(frc::Translation2d(units::meter_t{point.nextControl.x}, + // units::meter_t{point.nextControl.y}), 0_deg); frc::Pose2d p2 = + // frc::Pose2d(frc::Translation2d(units::meter_t{point.nextControl.x}, + // units::meter_t{point.prevControl.y}), 0_deg); + // + // _poses.emplace_back(p); + // + // WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("pathplanner/" + std::to_string(i) + + // "/poses/current"), p); WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("pathplanner/" + + // std::to_string(i) + "/poses/next"), p1); + // WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("pathplanner/" + std::to_string(i) + + // "/poses/prev"), p2); + // + // i++; + // } + + // i = 0; + for (const frc::Pose2d pose : _poses) { + WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("pathplanner/poses/" + std::to_string(i)), + pose); + // i++; + } + + _pathName = path; + + // _swerve->TurnToAngle(_poses[_currentPose].Rotation().Radians()); +} + +std::vector utils::FollowPath::CheckPoints(std::vector points) { + units::meter_t threshhold = 4_m; + bool posesgood = true; + + for (auto point : points) { + if (std::abs(point.position.X().value()) > threshhold.value() || std::abs(point.position.Y().value()) > threshhold.value()) { + posesgood = false; + } + } + + if (!posesgood) { + std::vector _points = + pathplanner::PathPlannerPath::fromPathFile(_pathName)->getAllPathPoints(); + + return CheckPoints(_points); + } + + return points; +} + +void utils::FollowPath::CalcTimer() { + frc::Pose2d current_pose = _swerve->GetPose(); + frc::Pose2d target_pose = _poses[_currentPose]; + + units::meter_t deltaX = target_pose.Translation().X() - current_pose.Translation().X(); + units::meter_t deltaY = target_pose.Translation().Y() - current_pose.Translation().Y(); + + units::meter_t dist = units::meter_t{std::sqrt(std::pow(deltaX.value(), 2) + std::pow(deltaY.value(), 2))}; + + _timer.Stop(); + _timer.Reset(); + _time = units::second_t{std::abs(dist.value()) / 1 /*meters per second ?No?*/}; + + _time = units::math::min(_time, 4_s); + + // _time = 15_s; + _timer.Start(); +} + +void utils::FollowPath::OnStart() { + _timer.Reset(); + + CalcTimer(); + + _timer.Start(); +} + +void utils::FollowPath::OnTick(units::second_t dt) { + nt::NetworkTableInstance::GetDefault() + .GetTable("pathplanner") + ->GetEntry("atPose") + .SetBoolean(_swerve->IsAtSetPose()); + nt::NetworkTableInstance::GetDefault() + .GetTable("pathplanner") + ->GetEntry("timeout") + .SetDouble(_time.value()); + nt::NetworkTableInstance::GetDefault() + .GetTable("pathplanner") + ->GetEntry("currentTime") + .SetDouble(_timer.Get().value()); + + WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("pathplanner/targetPose"), + _poses[_currentPose]); + WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("pathplanner/currentPose"), + _swerve->GetPose()); + nt::NetworkTableInstance::GetDefault() + .GetTable("pathplanner") + ->GetEntry("currentPoseNumber") + .SetInteger(_currentPose); + nt::NetworkTableInstance::GetDefault() + .GetTable("pathplanner") + ->GetEntry("amtPoses") + .SetInteger(static_cast(_poses.size())); + + #ifdef DEBUG + std::cout << "Following Path" << std::endl; + #endif + _poses[_currentPose] = frc::Pose2d(_poses[_currentPose].X(), _poses[_currentPose].Y(), _swerve->GetPose().Rotation()); + + // if (_swerve->IsAtSetAngle() && _swerve->GetState() == drivetrain::SwerveDriveState::kAngle) { + _swerve->SetPose(_poses[_currentPose]); + // } + /*else*/ if (_swerve->IsAtSetPose() || _timer.Get() > _time) { + if (_currentPose == static_cast(_poses.size())) { + // _swerve->MakeAtSetPoint(); + // _swerve->SetVelocity(frc::ChassisSpeeds()); + // _swerve->MakeAtSetPoint(); + SetDone(); + } else { + _currentPose++; + // _swerve->TurnToAngle(_poses[_currentPose].Rotation().Radians()); + CalcTimer(); + } + } + + std::string filePath = frc::filesystem::GetDeployDirectory() + "/pathplanner/paths/" + _pathName + ".path"; + + #ifdef DEBUG + std::cout << filePath << std::endl; + + nt::NetworkTableInstance::GetDefault() + .GetTable("pathplanner") + ->GetEntry("Current path") + .SetString(filePath); + #endif +} + +// AutoBuilder implementation +utils::AutoBuilder::AutoBuilder(drivetrain::SwerveDrive* swerve, std::function shouldFlipPath, + std::string auto_routine, utils::AutoCommands commands) + : _path(auto_routine), _flip(shouldFlipPath()), _swerve(swerve), _commandsList(std::move(commands)) { + SetAuto(auto_routine); +} + +void utils::AutoBuilder::SetAuto(std::string path) { + fs::path deploy_directory = frc::filesystem::GetDeployDirectory(); + + path = path + ".auto"; + + fs::path location = deploy_directory / "pathplanner" / "autos" / path; + + #ifdef DEBUG + std::cout << location << std::endl; + #endif + + std::ifstream file(location); + + std::string cdata; + std::string cjson; + + while (file.good()) { + file >> cdata; + cjson += cdata; + } + + // cjson.pop_back(); + + #ifdef DEBUG + std::cout << cjson << std::endl; + #endif + + wpi::json j = wpi::json::parse(cjson); + + _currentPath = &j; + _startingPose = &j["startingPose"]; + _commands = &j["command"]["data"]["commands"]; + + commands = std::vector>(); + + #ifdef DEBUG + nt::NetworkTableInstance::GetDefault().GetTable("json")->GetEntry("data").SetString(_currentPath->dump()); + // nt::NetworkTableInstance::GetDefault().GetTable("json")->GetEntry("start").SetString(_startingPose->dump()); + nt::NetworkTableInstance::GetDefault().GetTable("json")->GetEntry("commands").SetString(_commands->dump()); + nt::NetworkTableInstance::GetDefault() + .GetTable("json") + ->GetEntry("commands_type") + .SetString(_commands->type_name()); + #endif + + for (auto c : *_commands) { + if (c["type"] == "path") { + commands.push_back(std::make_pair(c["type"], c["data"]["pathName"])); + } + if (c["type"] == "named") { + commands.push_back(std::make_pair(c["type"], c["data"]["name"])); + } + if (c["type"] == "parallel") { + commands.push_back(std::make_pair(c["type"], "")); + } + } + + #ifdef DEBUG + nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("length").SetInteger(commands.size()); + #endif + + auto _pathplan = behaviour::make(); + + int i = 0; + int pathamt = 0; + int commandamt = 0; + + frc::Pose2d startPose; + + if (!_startingPose->is_null()) { + // startPose = JSONPoseToPose2d(*_startingPose); + startPose = frc::Pose2d(); + } else { + startPose = frc::Pose2d(); + } + + for (auto command : *_commands) { + + #ifdef DEBUG + nt::NetworkTableInstance::GetDefault() + .GetTable("commands/" + std::to_string(i)) + ->GetEntry("type") + .SetString(static_cast(command["type"])); + nt::NetworkTableInstance::GetDefault() + .GetTable("commands/" + std::to_string(i)) + ->GetEntry("data") + .SetString(static_cast(command["data"].dump())); + #endif + + if (command["type"] == "path") { + _pathplan->Add(behaviour::make(_swerve, command["data"]["pathName"], _flip, startPose)); + pathamt++; + #ifdef DEBUG + nt::NetworkTableInstance::GetDefault() + .GetTable("commands/" + std::to_string(i)) + ->GetEntry("behaviours") + .SetStringArray(_pathplan->GetQueue()); + #endif + } else if (command["type"] == "named") { + _pathplan->Add(_commandsList.Run(command["data"]["name"])); + commandamt++; + #ifdef DEBUG + nt::NetworkTableInstance::GetDefault() + .GetTable("commands/" + std::to_string(i)) + ->GetEntry("behaviours") + .SetStringArray(_pathplan->GetQueue()); + #endif + } else if (command["type"] == "parallel") { + auto nb = behaviour::make(behaviour::ConcurrentBehaviourReducer::ANY); + int j = 0; + for (auto c : command["data"]["commands"]) { + #ifdef DEBUG + nt::NetworkTableInstance::GetDefault() + .GetTable("commands/parallel/" + std::to_string(j)) + ->GetEntry("type") + .SetString(static_cast(c["type"])); + + nt::NetworkTableInstance::GetDefault() + .GetTable("commands/parallel/" + std::to_string(j)) + ->GetEntry("typeisstring") + .SetBoolean(c["type"].is_string()); + #endif + if (static_cast(c["type"]) == "path") { + nb->Add(behaviour::make(_swerve, c["data"]["pathName"], _flip, startPose)); + pathamt++; + } else if (static_cast(c["type"]) == "named") { + nb->Add(_commandsList.Run(c["data"]["name"])); + commandamt++; + } + nb->Add(behaviour::make("ok")); + j++; + } + #ifdef DEBUG + nt::NetworkTableInstance::GetDefault() + .GetTable("commands") + ->GetEntry("parallelcommandsamt") + .SetInteger(j); + nt::NetworkTableInstance::GetDefault() + .GetTable("commands") + ->GetEntry("parallel-" + std::to_string(i)) + .SetStringArray(nb->GetQueue()); + #endif + _pathplan->Add(nb); + } + + _pathplan->Add(behaviour::make("idk")); + + pathplan = _pathplan; + #ifdef DEBUG + nt::NetworkTableInstance::GetDefault() + .GetTable("commands/" + std::to_string(i)) + ->GetEntry("currentbehaviours") + .SetStringArray(pathplan->GetQueue()); + #endif + i++; + } + + // pathplan->Add(behaviour::make("test")); + // pathplan->Add(behaviour::make("test")); + + #ifdef DEBUG + nt::NetworkTableInstance::GetDefault() + .GetTable("commands/newbehaviours") + ->GetEntry(std::to_string(i)) + .SetStringArray(pathplan->GetQueue()); + + nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("PathAmt").SetInteger(pathamt); + nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("CommandAmt").SetInteger(commandamt); + #endif + + _swerve->SetAccelerationLimit(units::meters_per_second_squared_t{2}); + _swerve->SetVoltageLimit(6_V); + + // WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("startPose"), + // JSONPoseToPose2d(*_startingPose)); + + // nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("behaviours").SetStringArray(pathplan->GetName()); +} + +void utils::AutoBuilder::Invert() { + // Implement inversion logic if needed +} + +frc::Pose2d utils::AutoBuilder::JSONPoseToPose2d(wpi::json j) { + // std::cout << j["position"].is_object() << std::endl; + // std::cout << j << std::endl; + + #ifdef DEBUG + std::cout << j.dump() << std::endl; + #endif + + return frc::Pose2d(units::meter_t{j["position"]["x"]}, units::meter_t{j["position"]["y"]}, + units::degree_t{j["rotation"]}); + // return frc::Pose2d(); +} + +std::shared_ptr utils::AutoBuilder::GetAutoPath() { + #ifdef DEBUG + std::cout << "Getting Path" << std::endl; + #endif + + return pathplan; +} + +std::shared_ptr utils::AutoBuilder::GetAutoPath(std::string path) { + SetAuto(path); + return GetAutoPath(); +} + +// template +// behaviour::Behaviour utils::AutoBuilder::GetPath(std::string path) { +// Implement the logic to return the appropriate behaviour +// You might want to convert it to shared_ptr if needed +// return behaviour::Behaviour(); // Placeholder +// } + +// SwerveAutoBuilder implementation +utils::SwerveAutoBuilder::SwerveAutoBuilder(drivetrain::SwerveDrive* swerve, std::string name, + utils::AutoCommands commands) + : _builder(new AutoBuilder( + swerve, + []() { + // auto alliance = frc::DriverStation::GetAlliance(); + // return alliance && alliance.value() == frc::DriverStation::Alliance::kRed; + return false; + }, + name, std::move(commands))), + _swerve(swerve), + _name(name) {} + +void utils::SwerveAutoBuilder::SetAuto(std::string path) { + _builder->SetAuto(path); +} + +std::shared_ptr utils::SwerveAutoBuilder::GetAutoRoutine() { + return _builder->GetAutoPath(); +} + +std::shared_ptr utils::SwerveAutoBuilder::GetAutoRoutine(std::string path) { + return _builder->GetAutoPath(path); +} + +drivetrain::SwerveDrive* utils::SwerveAutoBuilder::GetSwerve() { + return _swerve; +} diff --git a/wombat/src/main/cpp/utils/Util.cpp b/wombat/src/main/cpp/utils/Util.cpp index e529e245..dbdbb642 100644 --- a/wombat/src/main/cpp/utils/Util.cpp +++ b/wombat/src/main/cpp/utils/Util.cpp @@ -13,21 +13,18 @@ units::second_t wom::utils::now() { return static_cast(now) / 1000000 * 1_s; } -void wom::utils::WritePose2NT(std::shared_ptr table, - frc::Pose2d pose) { +void wom::utils::WritePose2NT(std::shared_ptr table, frc::Pose2d pose) { table->GetEntry("x").SetDouble(pose.X().value()); table->GetEntry("y").SetDouble(pose.Y().value()); table->GetEntry("angle").SetDouble(pose.Rotation().Degrees().value()); } -void wom::utils::WritePose3NT(std::shared_ptr table, - frc::Pose3d pose) { +void wom::utils::WritePose3NT(std::shared_ptr table, frc::Pose3d pose) { table->GetEntry("x").SetDouble(pose.X().value()); table->GetEntry("y").SetDouble(pose.Y().value()); table->GetEntry("z").SetDouble(pose.Z().value()); - table->GetEntry("angle").SetDouble( - pose.Rotation().Z().convert().value()); + table->GetEntry("angle").SetDouble(pose.Rotation().Z().convert().value()); } double wom::utils::deadzone(double val, double deadzone) { @@ -50,32 +47,24 @@ units::volt_t wom::utils::GetVoltage(frc::MotorController* controller) { return controller->Get() * frc::RobotController::GetBatteryVoltage(); } -void wom::utils::WriteTrajectory(std::shared_ptr table, - frc::Trajectory trajectory) { +void wom::utils::WriteTrajectory(std::shared_ptr table, frc::Trajectory trajectory) { table->GetEntry("length").SetDouble(trajectory.TotalTime().value()); // write the trajectory to the network table int i = 0; for (auto state : trajectory.States()) { - table->GetSubTable(std::to_string(i)) - ->GetEntry("x") - .SetDouble(state.pose.X().value()); - table->GetSubTable(std::to_string(i)) - ->GetEntry("y") - .SetDouble(state.pose.Y().value()); + table->GetSubTable(std::to_string(i))->GetEntry("x").SetDouble(state.pose.X().value()); + table->GetSubTable(std::to_string(i))->GetEntry("y").SetDouble(state.pose.Y().value()); table->GetSubTable(std::to_string(i)) ->GetEntry("angle") .SetDouble(state.pose.Rotation().Degrees().value()); - table->GetSubTable(std::to_string(i)) - ->GetEntry("time") - .SetDouble(state.t.value()); + table->GetSubTable(std::to_string(i))->GetEntry("time").SetDouble(state.t.value()); i++; } } -void wom::utils::WriteTrajectoryState(std::shared_ptr table, - frc::Trajectory::State state) { +void wom::utils::WriteTrajectoryState(std::shared_ptr table, frc::Trajectory::State state) { table->GetEntry("x").SetDouble(state.pose.X().value()); table->GetEntry("y").SetDouble(state.pose.Y().value()); table->GetEntry("angle").SetDouble(state.pose.Rotation().Degrees().value()); diff --git a/wombat/src/main/cpp/vision/Camera.cpp b/wombat/src/main/cpp/vision/Camera.cpp new file mode 100644 index 00000000..da257f8e --- /dev/null +++ b/wombat/src/main/cpp/vision/Camera.cpp @@ -0,0 +1,107 @@ +// Copyright (c) 2023-2024 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + +#include "vision/Camera.h" + +#include +#include + +#include "frc/geometry/Transform3d.h" + +namespace wom { +namespace vision { +PhotonVision::PhotonVision() : _name("photonvision") { + Initialize(); +} + +PhotonVision::PhotonVision(std::string name) : _name(name) { + Initialize(); +} + +void PhotonVision::Initialize() { + _camera = new photonlib::PhotonCamera(_name); + + SetMode(_mode); +} + +photonlib::PhotonPipelineResult PhotonVision::GetResult() { + return _camera->GetLatestResult(); +} + +void PhotonVision::SetLED(PhotonVisionLEDMode mode) { + _camera->SetLEDMode(mode); +} + +void PhotonVision::SetPipelineIndex(int index) { + _camera->SetPipelineIndex(index); +} + +bool PhotonVision::HasTarget() { + return GetResult().HasTargets(); +} + +std::vector PhotonVision::GetTargets() { + const std::span res = GetResult().GetTargets(); + + std::vector targets; + + for (size_t i = 0; i < res.size(); i++) { + targets.push_back(res[i]); + } + + return targets; +} + +PhotonTarget PhotonVision::GetTarget() { + return GetResult().GetBestTarget(); +} + +double PhotonVision::GetTargetYaw(PhotonTarget target) { + return target.GetYaw(); +} + +double PhotonVision::GetTargetPitch(PhotonTarget target) { + return target.GetPitch(); +} + +double PhotonVision::GetTargetArea(PhotonTarget target) { + return target.GetArea(); +} + +double PhotonVision::GetTargetSkew(PhotonTarget target) { + return target.GetSkew(); +} + +frc::Transform3d PhotonVision::GetCameraToTarget(PhotonTarget target) { + return target.GetBestCameraToTarget(); +} + +std::vector> PhotonVision::GetTargetCorners(PhotonTarget target) { + return target.GetDetectedCorners(); +} + +int PhotonVision::GetTargetId(PhotonTarget target) { + return target.GetFiducialId(); +} + +frc::Transform3d PhotonVision::BestCameraToTarget(PhotonTarget target) { + return target.GetBestCameraToTarget(); +} + +frc::Transform3d PhotonVision::AlternateCameraToTarget(PhotonTarget target) { + return target.GetAlternateCameraToTarget(); +} + +PhotonVisionModes PhotonVision::GetMode() { + return _mode; +} + +void PhotonVision::SetMode(PhotonVisionModes mode) { + _mode = mode; + + SetPipelineIndex(static_cast(mode)); +} + +} // namespace vision +} // namespace wom diff --git a/wombat/src/main/cpp/vision/Limelight.cpp b/wombat/src/main/cpp/vision/Limelight.cpp index b37148ce..1bf77cd1 100644 --- a/wombat/src/main/cpp/vision/Limelight.cpp +++ b/wombat/src/main/cpp/vision/Limelight.cpp @@ -8,8 +8,7 @@ #include "utils/Util.h" -wom::vision::Limelight::Limelight(std::string limelightName) - : _limelightName(limelightName) {} +wom::vision::Limelight::Limelight(std::string limelightName) : _limelightName(limelightName) {} std::string wom::vision::Limelight::GetName() { return _limelightName; @@ -24,8 +23,7 @@ std::pair wom::vision::Limelight::GetOffset() { return offset; } -std::vector wom::vision::Limelight::GetAprilTagData( - LimelightAprilTagData dataType) { +std::vector wom::vision::Limelight::GetAprilTagData(LimelightAprilTagData dataType) { std::string dataName; switch (dataType) { @@ -60,17 +58,12 @@ std::vector wom::vision::Limelight::GetAprilTagData( case LimelightAprilTagData::kCamerapose_robotspace: dataName = "camerapose_robotspace"; break; - - case LimelightAprilTagData::kTid: - dataName = "tid"; - break; } - return table->GetNumberArray(dataName, std::vector(6)); + return table->GetEntry(dataName).GetDoubleArray(std::vector(6)); } -double wom::vision::Limelight::GetTargetingData(LimelightTargetingData dataType, - double defaultValue) { +double wom::vision::Limelight::GetTargetingData(LimelightTargetingData dataType, double defaultValue) { std::string dataName; switch (dataType) { @@ -129,9 +122,13 @@ double wom::vision::Limelight::GetTargetingData(LimelightTargetingData dataType, case LimelightTargetingData::kTc: dataName = "tc"; break; + + case LimelightTargetingData::kTid: + dataName = "tid"; + break; } - return table->GetNumber(dataName, defaultValue); + return table->GetEntry(dataName).GetDouble(0); } void wom::vision::Limelight::SetLEDMode(LimelightLEDMode mode) { @@ -158,8 +155,8 @@ void wom::vision::Limelight::SetCrop(std::array crop) { table->PutNumberArray("camtran", crop); } -units::meters_per_second_t wom::vision::Limelight::GetSpeed( - frc::Pose3d pose1, frc::Pose3d pose2, units::second_t dt) { +units::meters_per_second_t wom::vision::Limelight::GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2, + units::second_t dt) { frc::Transform3d dPose{pose1, pose2}; frc::Translation3d dTranslation = dPose.Translation(); @@ -170,26 +167,48 @@ units::meters_per_second_t wom::vision::Limelight::GetSpeed( return units::math::fabs(dTRANSLATION / dt); } +units::meters_per_second_t wom::vision::Limelight::GetSpeed(frc::Pose2d pose1, frc::Pose2d pose2, + units::second_t dt) { + frc::Transform2d dPose{pose1, pose2}; + frc::Translation2d dTranslation = dPose.Translation(); + + units::meter_t y = dTranslation.Y(); + units::meter_t x = dTranslation.X(); + units::radian_t theta = units::math::atan(y / x); + units::meter_t dTRANSLATION = x / units::math::cos(theta); + return units::math::fabs(dTRANSLATION / dt); +} + frc::Pose3d wom::vision::Limelight::GetPose() { std::vector pose = GetAprilTagData(LimelightAprilTagData::kBotpose); - return frc::Pose3d( - pose[1] * 1_m, 1_m * pose[2], 1_m * pose[3], - frc::Rotation3d(1_deg * (pose[4]), 1_deg * (pose[5]), 1_deg * (pose[6]))); + return frc::Pose3d(pose[1] * 1_m, 1_m * pose[2], 1_m * pose[3], + frc::Rotation3d(1_deg * (pose[4]), 1_deg * (pose[5]), 1_deg * (pose[6]))); } void wom::vision::Limelight::OnStart() { + #ifdef DEBUG std::cout << "starting" << std::endl; + #endif } void wom::vision::Limelight::OnUpdate(units::time::second_t dt) { wom::utils::WritePose3NT(table, GetPose()); } -bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose, - units::second_t dt) { +bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose, units::second_t dt) { frc::Pose3d actualPose = GetPose(); frc::Pose3d relativePose = actualPose.RelativeTo(pose); - return (units::math::fabs(relativePose.X()) < 0.01_m && - units::math::fabs(relativePose.Y()) < 0.01_m && + return (units::math::fabs(relativePose.X()) < 0.01_m && units::math::fabs(relativePose.Y()) < 0.01_m && GetSpeed(pose, GetPose(), dt) < 1_m / 1_s); } + +bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose2d pose, units::second_t dt) { + frc::Pose2d actualPose = GetPose().ToPose2d(); + frc::Pose2d relativePose = actualPose.RelativeTo(pose); + return (units::math::fabs(relativePose.X()) < 0.01_m && units::math::fabs(relativePose.Y()) < 0.01_m && + GetSpeed(pose, GetPose().ToPose2d(), dt) < 1_m / 1_s); +} + +bool wom::vision::Limelight::HasTarget() { + return GetTargetingData(LimelightTargetingData::kTv) == 1.0; +} diff --git a/wombat/src/main/include/Wombat.h b/wombat/src/main/include/Wombat.h index 9d297c4f..d48f4611 100644 --- a/wombat/src/main/include/Wombat.h +++ b/wombat/src/main/include/Wombat.h @@ -7,25 +7,26 @@ #include "behaviour/Behaviour.h" #include "behaviour/BehaviourScheduler.h" #include "behaviour/HasBehaviour.h" -#include "drivetrain/Drivetrain.h" #include "drivetrain/SwerveDrive.h" #include "drivetrain/behaviours/SwerveBehaviours.h" +#include "sim/Sim.h" #include "subsystems/Arm.h" -#include "subsystems/Elevator.h" -#include "subsystems/Shooter.h" #include "utils/Encoder.h" #include "utils/Gearbox.h" #include "utils/PID.h" +#include "utils/Pathplanner.h" #include "utils/RobotStartup.h" #include "utils/Util.h" +#include "vision/Camera.h" #include "vision/Limelight.h" namespace wom { using namespace wom; -using namespace wom::utils; using namespace wom::subsystems; +using namespace wom::utils; using namespace wom::drivetrain; using namespace wom::drivetrain::behaviours; using namespace wom::vision; +using namespace wom::sim; using namespace behaviour; } // namespace wom diff --git a/wombat/src/main/include/behaviour/Behaviour.h b/wombat/src/main/include/behaviour/Behaviour.h index 4f2e357e..3ae2d75d 100644 --- a/wombat/src/main/include/behaviour/Behaviour.h +++ b/wombat/src/main/include/behaviour/Behaviour.h @@ -22,13 +22,7 @@ #include "HasBehaviour.h" namespace behaviour { -enum class BehaviourState { - INITIALISED, - RUNNING, - DONE, - TIMED_OUT, - INTERRUPTED -}; +enum class BehaviourState { INITIALISED, RUNNING, DONE, TIMED_OUT, INTERRUPTED }; class SequentialBehaviour; @@ -46,8 +40,7 @@ class Behaviour : public std::enable_shared_from_this { public: using ptr = std::shared_ptr; - Behaviour(std::string name = "", - units::time::second_t period = 20_ms); + explicit Behaviour(std::string name = "", units::time::second_t period = 20_ms); ~Behaviour(); /** @@ -189,20 +182,22 @@ class SequentialBehaviour : public Behaviour { void OnTick(units::time::second_t dt) override; void OnStop() override; + std::vector GetQueue(); + protected: - std::deque _queue; + // std::deque _queue; + std::vector _queue; }; -inline std::shared_ptr operator<<(Behaviour::ptr a, - Behaviour::ptr b) { +inline std::shared_ptr operator<<(Behaviour::ptr a, Behaviour::ptr b) { auto seq = std::make_shared(); seq->Add(a); seq->Add(b); return seq; } -inline std::shared_ptr operator<<( - std::shared_ptr a, Behaviour::ptr b) { +inline std::shared_ptr operator<<(std::shared_ptr a, + Behaviour::ptr b) { a->Add(b); return a; } @@ -235,6 +230,8 @@ class ConcurrentBehaviour : public Behaviour { void OnTick(units::time::second_t dt) override; void OnStop() override; + std::vector GetQueue(); + private: ConcurrentBehaviourReducer _reducer; std::vector> _children; @@ -247,10 +244,8 @@ class ConcurrentBehaviour : public Behaviour { * Create a concurrent behaviour group, waiting for all behaviours * to finish before moving on. */ -inline std::shared_ptr operator&(Behaviour::ptr a, - Behaviour::ptr b) { - auto conc = - std::make_shared(ConcurrentBehaviourReducer::ALL); +inline std::shared_ptr operator&(Behaviour::ptr a, Behaviour::ptr b) { + auto conc = std::make_shared(ConcurrentBehaviourReducer::ALL); conc->Add(a); conc->Add(b); return conc; @@ -261,10 +256,8 @@ inline std::shared_ptr operator&(Behaviour::ptr a, * be interrupted as soon as any members of the group are finished (the * behaviours are 'raced' against each other). */ -inline std::shared_ptr operator|(Behaviour::ptr a, - Behaviour::ptr b) { - auto conc = - std::make_shared(ConcurrentBehaviourReducer::ANY); +inline std::shared_ptr operator|(Behaviour::ptr a, Behaviour::ptr b) { + auto conc = std::make_shared(ConcurrentBehaviourReducer::ANY); conc->Add(a); conc->Add(b); return conc; @@ -332,8 +325,7 @@ struct Switch : public Behaviour { * @param condition The function yielding true if this is the correct option * @param b The behaviour to call if this option is provided. */ - std::shared_ptr When(std::function condition, - Behaviour::ptr b) { + std::shared_ptr When(std::function condition, Behaviour::ptr b) { _options.push_back(std::pair(condition, b)); Inherit(*b); return std::reinterpret_pointer_cast>(shared_from_this()); @@ -389,8 +381,7 @@ struct Switch : public Behaviour { private: std::function _fn; - wpi::SmallVector, Behaviour::ptr>, 4> - _options; + wpi::SmallVector, Behaviour::ptr>, 4> _options; Behaviour::ptr _locked = nullptr; }; @@ -407,10 +398,8 @@ struct Decide : public Switch { * @param condition The function yielding true if this is the correct option * @param b The behaviour to call if this option is provided. */ - std::shared_ptr When(std::function condition, - Behaviour::ptr b) { - return std::reinterpret_pointer_cast( - Switch::When([condition](auto) { return condition(); }, b)); + std::shared_ptr When(std::function condition, Behaviour::ptr b) { + return std::reinterpret_pointer_cast(Switch::When([condition](auto) { return condition(); }, b)); } }; diff --git a/wombat/src/main/include/behaviour/HasBehaviour.h b/wombat/src/main/include/behaviour/HasBehaviour.h index c9b35b2e..bae5ac05 100644 --- a/wombat/src/main/include/behaviour/HasBehaviour.h +++ b/wombat/src/main/include/behaviour/HasBehaviour.h @@ -30,8 +30,7 @@ class HasBehaviour { protected: std::shared_ptr _active_behaviour{nullptr}; - std::function(void)> _default_behaviour_producer{ - nullptr}; + std::function(void)> _default_behaviour_producer{nullptr}; private: friend class BehaviourScheduler; diff --git a/wombat/src/main/include/drivetrain/Drivetrain.h b/wombat/src/main/include/drivetrain/Drivetrain.h deleted file mode 100644 index 0b8fcc72..00000000 --- a/wombat/src/main/include/drivetrain/Drivetrain.h +++ /dev/null @@ -1,57 +0,0 @@ -// Copyright (c) 2023-2024 CurtinFRC -// Open Source Software, you can modify it according to the terms -// of the MIT License at the root of this project - -#pragma once - -#include -#include -#include - -#include - -#include "behaviour/HasBehaviour.h" -#include "utils/Gearbox.h" - -namespace wom { -namespace drivetrain { -// TODO PID -struct DrivetrainConfig { - std::string path; - - wom::utils::Gearbox left1; - wom::utils::Gearbox left2; - wom::utils::Gearbox left3; - wom::utils::Gearbox right1; - wom::utils::Gearbox right2; - wom::utils::Gearbox right3; -}; - -enum DrivetrainState { - kIdle, - kTank, - kAuto, -}; - -class Drivetrain : public behaviour::HasBehaviour { - public: - Drivetrain(DrivetrainConfig* config, frc::XboxController& driver); - ~Drivetrain(); - - DrivetrainConfig* GetConfig(); - DrivetrainState GetState(); - - void SetState(DrivetrainState state); - - void OnStart(); - void OnUpdate(units::second_t dt); - - protected: - private: - DrivetrainConfig* _config; - DrivetrainState _state; - frc::XboxController& _driver; - units::volt_t maxVolts = 9_V; -}; -} // namespace drivetrain -} // namespace wom diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h index afd135f8..3c7531e3 100644 --- a/wombat/src/main/include/drivetrain/SwerveDrive.h +++ b/wombat/src/main/include/drivetrain/SwerveDrive.h @@ -4,9 +4,9 @@ #pragma once +#include #include #include -#include #include #include #include @@ -16,6 +16,9 @@ #include #include #include +#include +#include +#include #include #include @@ -25,26 +28,17 @@ #include #include "behaviour/HasBehaviour.h" +#include "units/angle.h" #include "utils/Gearbox.h" #include "utils/PID.h" -#include -#include - -#include -#include -#include - - namespace wom { namespace drivetrain { /** * Implements a PID control loop. */ -class PIDController - : public wpi::Sendable, - public wpi::SendableHelper { +class PIDController : public wpi::Sendable, public wpi::SendableHelper { public: /** * Allocates a PIDController with the given constants for Kp, Ki, and Kd. @@ -55,8 +49,7 @@ class PIDController * @param period The period between controller updates in seconds. The * default is 20 milliseconds. Must be positive. */ - PIDController(double Kp, double Ki, double Kd, - units::second_t period = 20_ms); + PIDController(double Kp, double Ki, double Kd, units::second_t period = 20_ms); ~PIDController() override = default; @@ -219,9 +212,8 @@ class PIDController * @param positionTolerance Position error which is tolerable. * @param velocityTolerance Velocity error which is tolerable. */ - void SetTolerance( - double positionTolerance, - double velocityTolerance = std::numeric_limits::infinity()); + void SetTolerance(double positionTolerance, + double velocityTolerance = std::numeric_limits::infinity()); /** * Returns the difference between the setpoint and the measurement. @@ -294,7 +286,7 @@ class PIDController double m_totalError = 0; // The error that is considered at setpoint. - double m_positionTolerance = 0.05; + double m_positionTolerance = 0.2; double m_velocityTolerance = std::numeric_limits::infinity(); double m_setpoint = 0; @@ -304,7 +296,6 @@ class PIDController bool m_haveMeasurement = false; }; - enum class SwerveModuleState { kZeroing, kIdle, kPID }; enum class TurnOffsetValues { reverse, forward, none }; @@ -323,9 +314,8 @@ struct SwerveModuleConfig { class SwerveModule { public: - //using angle_pid_conf_t = utils::PIDConfig; - using velocity_pid_conf_t = - utils::PIDConfig; + // using angle_pid_conf_t = utils::PIDConfig; + using velocity_pid_conf_t = utils::PIDConfig; SwerveModule(std::string path, SwerveModuleConfig config, /*angle_pid_conf_t anglePID,*/ velocity_pid_conf_t velocityPID); @@ -342,8 +332,7 @@ class SwerveModule { void SetZero(units::second_t dt); void SetIdle(); - void SetPID(units::radian_t angle, units::meters_per_second_t speed, - units::second_t dt); + void SetPID(units::radian_t angle, units::meters_per_second_t speed, units::second_t dt); void SetZero(); void SetVoltageLimit(units::volt_t driveModuleVoltageLimit); @@ -363,13 +352,14 @@ class SwerveModule { const SwerveModuleConfig& GetConfig() const; - //utils::PIDController _anglePIDController; + // utils::PIDController _anglePIDController; frc::PIDController _anglePIDController; private: SwerveModuleConfig _config; SwerveModuleState _state; units::volt_t _driveModuleVoltageLimit = 10_V; + units::volt_t _angleModuleVoltageLimit = 6_V; bool _hasZeroedEncoder = false; bool _hasZeroed = false; @@ -389,20 +379,18 @@ class SwerveModule { struct SwerveDriveConfig { /*using pose_angle_conf_t = utils::PIDConfig;*/ - using pose_position_conf_t = - utils::PIDConfig; - using balance_conf_t = - utils::PIDConfig; + using pose_position_conf_t = utils::PIDConfig; + using balance_conf_t = utils::PIDConfig; std::string path; - //SwerveModule::angle_pid_conf_t anglePID; + // SwerveModule::angle_pid_conf_t anglePID; SwerveModule::velocity_pid_conf_t velocityPID; wpi::array modules; ctre::phoenix6::hardware::Pigeon2* gyro; - //pose_angle_conf_t poseAnglePID; + // pose_angle_conf_t poseAnglePID; pose_position_conf_t posePositionPID; units::kilogram_t mass; @@ -416,6 +404,7 @@ struct SwerveDriveConfig { enum class SwerveDriveState { kZeroing, kIdle, + kAngle, kVelocity, kFieldRelativeVelocity, kPose, @@ -456,8 +445,7 @@ class SwerveDrive : public behaviour::HasBehaviour { * @brief This function switches the state to handle the robot's rotation * matching that of the joystick */ - void RotateMatchJoystick(units::radian_t joystickAngle, - FieldRelativeSpeeds speeds); + void RotateMatchJoystick(units::radian_t joystickAngle, FieldRelativeSpeeds speeds); void SetIdle(); @@ -467,12 +455,15 @@ class SwerveDrive : public behaviour::HasBehaviour { void SetFieldRelativeVelocity(FieldRelativeSpeeds speeds); void SetPose(frc::Pose2d pose); bool IsAtSetPose(); - void SetIndividualTuning(int mod, units::radian_t angle, - units::meters_per_second_t speed); + void SetIndividualTuning(int mod, units::radian_t angle, units::meters_per_second_t speed); void SetTuning(units::radian_t angle, units::meters_per_second_t speed); void SetZero(); void SetVoltageLimit(units::volt_t driveVoltageLimit); void OnResetMode(); + + SwerveDriveState GetState(); + bool IsAtSetAngle(); + // double GetModuleCANPosition(int mod); // from liam's void SetXWheelState(); @@ -487,8 +478,14 @@ class SwerveDrive : public behaviour::HasBehaviour { frc::Pose2d GetPose(); void AddVisionMeasurement(frc::Pose2d pose, units::second_t timestamp); + void TurnToAngle(units::radian_t angle); + SwerveDriveConfig& GetConfig() { return _config; } + frc::Pose2d GetSetpoint(); + + void MakeAtSetPoint(); + private: SwerveDriveConfig _config; SwerveDriveState _state = SwerveDriveState::kIdle; @@ -514,9 +511,12 @@ class SwerveDrive : public behaviour::HasBehaviour { /*utils::PIDController _anglePIDController;*/ - PIDController _anglePIDController; - utils::PIDController _xPIDController; - utils::PIDController _yPIDController; + wom::drivetrain::PIDController _anglePIDController; + wom::drivetrain::PIDController _xPIDController; + wom::drivetrain::PIDController _yPIDController; + wom::drivetrain::PIDController _turnPIDController; + // wom::utils::PIDController _xPIDController; + // wom::utils::PIDController _yPIDController; std::shared_ptr _table; @@ -526,7 +526,10 @@ class SwerveDrive : public behaviour::HasBehaviour { int _mod; units::radian_t _angle; units::meters_per_second_t _speed; + frc::SwerveModuleState laststates[4]; + frc::SwerveModuleState lastnewstates[4]; + units::radian_t _reqAngle; // double frontLeftEncoderOffset = -143.26171875; // double frontRightEncoderOffset = 167.87109375; // double backLeftEncoderOffset = -316.669921875; diff --git a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h index 0a274ea6..62a0c7d7 100644 --- a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h +++ b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h @@ -16,7 +16,10 @@ #include #include "behaviour/Behaviour.h" +#include "behaviour/HasBehaviour.h" #include "drivetrain/SwerveDrive.h" +#include "units/angle.h" +#include "units/time.h" #include "utils/Pathplanner.h" namespace wom { @@ -37,16 +40,14 @@ class ManualDrivebase : public behaviour::Behaviour { * A pointer to the controller that the driver has been allocated (the * allocated memory address that stores the "driver controller" object) */ - ManualDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase, - frc::XboxController* driverController); + ManualDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase, frc::XboxController* driverController); void OnTick(units::second_t deltaTime) override; /** * @brief This function handles all of the logic behind the tangent function, * to be able to calculate an angle between 0 andd 360 degrees, inclusively */ - void CalculateRequestedAngle(double joystickX, double joystickY, - units::degree_t defaultAngle); + void CalculateRequestedAngle(double joystickX, double joystickY, units::degree_t defaultAngle); void OnStart() override; void ResetMode(); @@ -72,8 +73,8 @@ class ManualDrivebase : public behaviour::Behaviour { const double turningDeadzone = 0.2; // Variables for solution to Anti-tip - double prevJoystickX, prevJoystickY, prevPrevJoystickX, prevPrevJoystickY, - usingJoystickXPos, usingJoystickYPos; + double prevJoystickX, prevJoystickY, prevPrevJoystickX, prevPrevJoystickY, usingJoystickXPos, + usingJoystickYPos; // The speed that the joystick must travel to activate averaging over previous // 3 joystick positions const double smoothingThreshold = 1; @@ -83,9 +84,9 @@ class ManualDrivebase : public behaviour::Behaviour { // The translation speeds for when "slow speed", "normal speed", "fast speed" // modes are active - const translationSpeed_ lowSensitivityDriveSpeed = 15_ft / 1_s; - const translationSpeed_ defaultDriveSpeed = 10_ft / 1_s; - const translationSpeed_ highSensitivityDriveSpeed = 10_ft / 1_s; + const translationSpeed_ lowSensitivityDriveSpeed = 80_ft / 1_s; + const translationSpeed_ defaultDriveSpeed = 80_ft / 1_s; + const translationSpeed_ highSensitivityDriveSpeed = 80_ft / 1_s; // The rotation speeds for when "slow speed", "normal speed", "fast speed" // modes are active const rotationSpeed_ lowSensitivityRotateSpeed = 120_deg / 1_s; @@ -124,83 +125,129 @@ class GoToPose : public behaviour::Behaviour { frc::Pose3d _pose; }; -class FollowTrajectory : public behaviour::Behaviour { - public: - FollowTrajectory(wom::drivetrain::SwerveDrive* swerve, - wom::utils::Pathplanner* pathplanner, std::string path); - - void OnTick(units::second_t dt) override; - - void OnStart() override; +// class FollowTrajectory : public behaviour::Behaviour { +// public: +// FollowTrajectory(wom::drivetrain::SwerveDrive* swerve, wom::utils::Pathplanner* pathplanner, +// std::string path); +// +// void OnTick(units::second_t dt) override; +// +// void OnStart() override; +// +// private: +// wom::utils::Pathplanner* _pathplanner; +// std::string _path; +// wom::drivetrain::SwerveDrive* _swerve; +// frc::Trajectory _trajectory; +// frc::Timer m_timer; +// }; +// +// class TempSimSwerveDrive { +// public: +// TempSimSwerveDrive(frc::Timer* timer, frc::Field2d* field); +// +// void OnUpdate(); +// +// void SetPath(std::string path); +// +// frc::Pose3d GetPose(); +// frc::Pose2d GetPose2d(); +// +// private: +// frc::sim::DifferentialDrivetrainSim m_driveSim{ +// frc::DCMotor::NEO(2), // 2 NEO motors on each side of the drivetrain. +// 7.29, // 7.29:1 gearing reduction. +// 7.5_kg_sq_m, // MOI of 7.5 kg m^2 (from CAD model). +// 60_kg, // The mass of the robot is 60 kg. +// 3_in, // The robot uses 3" radius wheels. +// 0.7112_m, // The track width is 0.7112 meters. +// +// // The standard deviations for measurement noise: +// // x and y: 0.001 m +// // heading: 0.001 rad +// // l and r velocity: 0.1 m/s +// // l and r position: 0.005 m +// {0.001, 0.001, 0.001, 0.1, 0.1, 0.005, 0.005}}; +// +// wom::utils::Pathplanner m_pathplanner; +// +// frc::Trajectory current_trajectory; +// +// std::shared_ptr current_trajectory_table; +// std::shared_ptr current_trajectory_state_table; +// +// frc::Timer* m_timer; +// +// frc::Field2d* m_field; +// +// std::string m_path; +// }; +// +// class AutoSwerveDrive { +// public: +// AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer, frc::Field2d* field); +// +// void OnUpdate(); +// +// void SetPath(std::string path); +// +// private: +// wom::drivetrain::SwerveDrive* _swerve; +// +// TempSimSwerveDrive* _simSwerveDrive; +// +// frc::Timer* m_timer; +// +// frc::Field2d* m_field; +// +// std::string m_path; +// }; - private: - wom::utils::Pathplanner* _pathplanner; - std::string _path; - wom::drivetrain::SwerveDrive* _swerve; - frc::Trajectory _trajectory; - frc::Timer m_timer; -}; - -class TempSimSwerveDrive { +/** + * @brief Behaviour Class to hangle the swerve drivebase going to and potentially maintaining the position + */ +class DrivebasePoseBehaviour : public behaviour::Behaviour { public: - TempSimSwerveDrive(frc::Timer* timer, frc::Field2d* field); - - void OnUpdate(); - - void SetPath(std::string path); + /** + * @param swerveDrivebase + * A pointer to the swerve drivebase + * @param pose + * A variable containing an X coordinate, a Y coordinate, and a rotation, for the drivebase to go to + * @param hold + * An optional variable (defaulting false), to say whether this position should be maintained + */ + DrivebasePoseBehaviour(SwerveDrive* swerveDrivebase, frc::Pose2d pose, units::volt_t voltageLimit = 10_V, + bool hold = false); - frc::Pose3d GetPose(); - frc::Pose2d GetPose2d(); + /** + * @brief + * + * @param deltaTime change in time since the last iteration + */ + void OnTick(units::second_t deltaTime) override; + void OnStart() override; private: - frc::sim::DifferentialDrivetrainSim m_driveSim{ - frc::DCMotor::NEO(2), // 2 NEO motors on each side of the drivetrain. - 7.29, // 7.29:1 gearing reduction. - 7.5_kg_sq_m, // MOI of 7.5 kg m^2 (from CAD model). - 60_kg, // The mass of the robot is 60 kg. - 3_in, // The robot uses 3" radius wheels. - 0.7112_m, // The track width is 0.7112 meters. - - // The standard deviations for measurement noise: - // x and y: 0.001 m - // heading: 0.001 rad - // l and r velocity: 0.1 m/s - // l and r position: 0.005 m - {0.001, 0.001, 0.001, 0.1, 0.1, 0.005, 0.005}}; - - wom::utils::Pathplanner m_pathplanner; - - frc::Trajectory current_trajectory; + SwerveDrive* _swerveDrivebase; + frc::Pose2d _pose; + bool _hold; + units::volt_t _voltageLimit; - std::shared_ptr current_trajectory_table; - std::shared_ptr current_trajectory_state_table; + frc::Timer _timer; - frc::Timer* m_timer; - - frc::Field2d* m_field; - - std::string m_path; + std::shared_ptr _swerveDriveTable = + nt::NetworkTableInstance::GetDefault().GetTable("swerve"); }; -class AutoSwerveDrive { - public: - AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer, - frc::Field2d* field); - - void OnUpdate(); - - void SetPath(std::string path); +class TurnToAngleBehaviour : public behaviour::Behaviour { +public: + TurnToAngleBehaviour(SwerveDrive* swerve, units::radian_t angle); - private: - wom::drivetrain::SwerveDrive* _swerve; - - TempSimSwerveDrive* _simSwerveDrive; - - frc::Timer* m_timer; - - frc::Field2d* m_field; + void OnTick(units::second_t dt) override; - std::string m_path; +private: + units::radian_t _angle; + SwerveDrive* _swerve; }; } // namespace behaviours } // namespace drivetrain diff --git a/wombat/src/main/include/sim/Sim.h b/wombat/src/main/include/sim/Sim.h new file mode 100644 index 00000000..2fb92c86 --- /dev/null +++ b/wombat/src/main/include/sim/Sim.h @@ -0,0 +1,25 @@ +// Copyright (c) 2023-2024 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + +#pragma once + +#include "drivetrain/SwerveDrive.h" +#include "frc/geometry/Pose2d.h" +#include "frc/smartdashboard/Field2d.h" + +namespace wom { +namespace sim { +class SimSwerve { + public: + explicit SimSwerve(drivetrain::SwerveDrive* _swerve); + + void OnTick(); + void OnTick(frc::Pose2d pose); + + private: + frc::Field2d _field; + drivetrain::SwerveDrive* _swerve; +}; +} // namespace sim +} // namespace wom diff --git a/wombat/src/main/include/subsystems/Arm.h b/wombat/src/main/include/subsystems/Arm.h index 155fd4cd..75ec4b00 100644 --- a/wombat/src/main/include/subsystems/Arm.h +++ b/wombat/src/main/include/subsystems/Arm.h @@ -14,6 +14,7 @@ #include #include "behaviour/HasBehaviour.h" +#include "utils/Encoder.h" #include "utils/Gearbox.h" #include "utils/PID.h" @@ -24,7 +25,7 @@ struct ArmConfig { wom::utils::Gearbox leftGearbox; wom::utils::Gearbox rightGearbox; - wom::utils::CANSparkMaxEncoder armEncoder; + wom::utils::CANSparkMaxEncoder* armEncoder; wom::utils::PIDConfig pidConfig; wom::utils::PIDConfig velocityConfig; @@ -57,6 +58,7 @@ class Arm : public behaviour::HasBehaviour { void SetArmSpeedLimit(double limit); // units, what are they?? ArmConfig& GetConfig(); + ArmState GetState(); units::radian_t GetAngle() const; units::radians_per_second_t MaxSpeed() const; @@ -68,8 +70,7 @@ class Arm : public behaviour::HasBehaviour { ArmConfig _config; ArmState _state = ArmState::kIdle; wom::utils::PIDController _pid; - wom::utils::PIDController - _velocityPID; + wom::utils::PIDController _velocityPID; std::shared_ptr _table; diff --git a/wombat/src/main/include/subsystems/Shooter.h b/wombat/src/main/include/subsystems/Shooter.h index 2bbf6f5e..e5ac219d 100644 --- a/wombat/src/main/include/subsystems/Shooter.h +++ b/wombat/src/main/include/subsystems/Shooter.h @@ -62,8 +62,7 @@ class ShooterConstant : public behaviour::Behaviour { class ShooterSpinup : public behaviour::Behaviour { public: - ShooterSpinup(Shooter* s, units::radians_per_second_t speed, - bool hold = false); + ShooterSpinup(Shooter* s, units::radians_per_second_t speed, bool hold = false); void OnTick(units::second_t dt) override; diff --git a/wombat/src/main/include/utils/Encoder.h b/wombat/src/main/include/utils/Encoder.h index d17ba4e2..8d06d725 100644 --- a/wombat/src/main/include/utils/Encoder.h +++ b/wombat/src/main/include/utils/Encoder.h @@ -125,4 +125,4 @@ class CanEncoder : public Encoder { ctre::phoenix6::hardware::CANcoder* _canEncoder; }; } // namespace utils -} // namespace wom \ No newline at end of file +} // namespace wom diff --git a/wombat/src/main/include/utils/PID.h b/wombat/src/main/include/utils/PID.h index 2ce378ec..dcb9c766 100644 --- a/wombat/src/main/include/utils/PID.h +++ b/wombat/src/main/include/utils/PID.h @@ -24,19 +24,15 @@ struct PIDConfig { using in_t = units::unit_t; using kp_t = units::unit_t>>; - using ki_t = - units::unit_t, - units::inverse>>; - using kd_t = units::unit_t< - units::compound_unit, units::second>>; + using ki_t = units::unit_t, units::inverse>>; + using kd_t = units::unit_t, units::second>>; using error_t = units::unit_t; - using deriv_t = - units::unit_t>>; + using deriv_t = units::unit_t>>; - PIDConfig(std::string path, kp_t kp = kp_t{0}, ki_t ki = ki_t{0}, - kd_t kd = kd_t{0}, error_t stableThresh = error_t{-1}, - deriv_t stableDerivThresh = deriv_t{-1}, in_t izone = in_t{-1}) + PIDConfig(std::string path, kp_t kp = kp_t{0}, ki_t ki = ki_t{0}, kd_t kd = kd_t{0}, + error_t stableThresh = error_t{-1}, deriv_t stableDerivThresh = deriv_t{-1}, + in_t izone = in_t{-1}) : path(path), kp(kp), ki(ki), @@ -64,23 +60,14 @@ struct PIDConfig { public: void RegisterNT() { auto table = nt::NetworkTableInstance::GetDefault().GetTable(path); + _nt_bindings.emplace_back(std::make_shared>(table, "kP", kp)); + _nt_bindings.emplace_back(std::make_shared>(table, "kI", ki)); + _nt_bindings.emplace_back(std::make_shared>(table, "kD", kd)); _nt_bindings.emplace_back( - std::make_shared>(table, "kP", - kp)); - _nt_bindings.emplace_back( - std::make_shared>(table, "kI", - ki)); - _nt_bindings.emplace_back( - std::make_shared>(table, "kD", - kd)); - _nt_bindings.emplace_back( - std::make_shared>( - table, "stableThresh", stableThresh)); - _nt_bindings.emplace_back( - std::make_shared>( - table, "stableThreshVelocity", stableDerivThresh)); - _nt_bindings.emplace_back( - std::make_shared>(table, "izone", izone)); + std::make_shared>(table, "stableThresh", stableThresh)); + _nt_bindings.emplace_back(std::make_shared>( + table, "stableThreshVelocity", stableDerivThresh)); + _nt_bindings.emplace_back(std::make_shared>(table, "izone", izone)); } }; @@ -94,19 +81,15 @@ class PIDController { config_t config; - PIDController(std::string path, config_t initialGains, - in_t setpoint = in_t{0}) + PIDController(std::string path, config_t initialGains, in_t setpoint = in_t{0}) : config(initialGains), _setpoint(setpoint), - _posFilter( - frc::LinearFilter::MovingAverage(20)), - _velFilter( - frc::LinearFilter::MovingAverage(20)), + _posFilter(frc::LinearFilter::MovingAverage(20)), + _velFilter(frc::LinearFilter::MovingAverage(20)), _table(nt::NetworkTableInstance::GetDefault().GetTable(path)) {} void SetSetpoint(in_t setpoint) { - if (std::abs(setpoint.value() - _setpoint.value()) > - std::abs(0.1 * _setpoint.value())) { + if (std::abs(setpoint.value() - _setpoint.value()) > std::abs(0.1 * _setpoint.value())) { _iterations = 0; } _setpoint = setpoint; @@ -121,13 +104,17 @@ class PIDController { void Reset() { _integralSum = sum_t{0}; } out_t Calculate(in_t pv, units::second_t dt, out_t feedforward = out_t{0}) { - pv = units::math::fabs(pv); + // pv = units::math::fabs(pv); + bool is_negative; + if (pv.value() < 0) { + is_negative = true; + pv = units::math::fabs(pv); + } auto error = do_wrap(_setpoint - pv); error = units::math::fabs(error); _integralSum += error * dt; _integralSum = units::math::fabs(_integralSum); - if (config.izone.value() > 0 && - (error > config.izone || error < -config.izone)) + if (config.izone.value() > 0 && (error > config.izone || error < -config.izone)) _integralSum = sum_t{0}; typename config_t::deriv_t deriv{0}; @@ -138,19 +125,24 @@ class PIDController { _stablePos = _posFilter.Calculate(error); _stableVel = _velFilter.Calculate(deriv); - auto out = config.kp * error + config.ki * _integralSum + - config.kd * deriv + feedforward; + auto out = config.kp * error + config.ki * _integralSum + config.kd * deriv + feedforward; _last_pv = pv; _last_error = error; _iterations++; + if (is_negative) { + out *= -1; + } + +#ifdef DEBUG + _table->GetEntry("error").SetDouble(out.value()); +#endif + return out; } - bool IsStable( - std::optional stableThreshOverride = {}, - std::optional velocityThreshOverride = {}) - const { + bool IsStable(std::optional stableThreshOverride = {}, + std::optional velocityThreshOverride = {}) const { auto stableThresh = config.stableThresh; auto stableDerivThresh = config.stableDerivThresh; @@ -159,10 +151,8 @@ class PIDController { if (velocityThreshOverride.has_value()) stableDerivThresh = velocityThreshOverride.value(); - return _iterations > 20 && - std::abs(_stablePos.value()) <= std::abs(stableThresh.value()) && - (stableDerivThresh.value() < 0 || - std::abs(_stableVel.value()) <= stableDerivThresh.value()); + return _iterations > 20 && std::abs(_stablePos.value()) <= std::abs(stableThresh.value()) && + (stableDerivThresh.value() < 0 || std::abs(_stableVel.value()) <= stableDerivThresh.value()); } private: diff --git a/wombat/src/main/include/utils/Pathplanner.h b/wombat/src/main/include/utils/Pathplanner.h index 48e760ae..d2481543 100644 --- a/wombat/src/main/include/utils/Pathplanner.h +++ b/wombat/src/main/include/utils/Pathplanner.h @@ -8,16 +8,319 @@ #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "behaviour/Behaviour.h" +#include "drivetrain/SwerveDrive.h" +#include "frc/Timer.h" +#include "pathplanner/lib/path/PathPlannerPath.h" +#include "units/time.h" +#include "wpi/json_fwd.h" + namespace wom { namespace utils { -class Pathplanner { + +struct BezierPoint { + struct { + double x; + double y; + } anchor; + + struct { + double x; + double y; + } prevControl; + + struct { + double x; + double y; + } nextControl; + + bool isLocked; + std::string linkedName; + + BezierPoint(double anchorX, double anchorY, wpi::json prevControlX, wpi::json prevControlY, + wpi::json nextControlX, wpi::json nextControlY, bool isLocked, wpi::json linkedName); + + // Function to calculate the angle of the point relative to the x-axis + double calculateAngle() const; +}; + +// Function to calculate the distance between two points +double distance(const BezierPoint& p1, const BezierPoint& p2); + +// Function to interpolate points at regular intervals +std::vector interpolateAtIntervals(const BezierPoint& p1, const BezierPoint& p2, + double interval); + +// Function to create a list of BezierPoints from JSON data +std::vector createBezierPointsFromJson(const wpi::json& jsonData); + +// Function to create a list of interpolated BezierPoints +std::vector interpolateBezierPoints(const std::vector& bezierPoints, + double interval); + +// template +// class ConcreteCommand; + +// template +// class CommandBase { +// public: +// virtual ~CommandBase() = default; +// virtual std::shared_ptr Execute(Args...) const = 0; +// virtual bool IsValid(std::vector argTypes) const = 0; +// virtual std::string GetName() const = 0; +// }; + +// template +// class Command : public CommandBase{ +// public: +// using FunctionType = std::function; +// +// Command(std::string name, FunctionType func); + +// std::shared_ptr Execute(Args... args) const; +// Ret Execute(Args... args) const; + +// std::string GetName() const override; + +// bool IsValid(std::vector argTypes) const; + +// private: +// std::string name_; +// FunctionType function_; +// std::vector argumentTypes_; +// }; +// template +// class Command { +// public: +// using BehaviorPtr = std::shared_ptr; +// +// Command(const std::string& name, std::function func) +// : name_(name), func_(func) {} +// +// BehaviorPtr Execute() const { +// return func_(); +// } +// +// const std::string& GetName() const { +// return name_; +// } +// +// bool IsValid() const { +// return func_ != nullptr; +// } +// +// private: +// std::string name_; +// std::function func_; +// }; +// +// class Commands { +// public: +// template +// Commands(CommandArgs&&... commands); +// +// template +// Ret Execute(std::string command, Args&&... args); +// +// bool IsValid(std::string command, std::vector argTypes) const; +// +// template +// Ret Run(std::string commandString, Args&&... args); +// +// private: +// std::tuple commands_; +// +// std::vector ParseArguments(const std::string& argString); +// std::any ParseSingleArgument(const std::string& arg); +// +// template +// static std::type_index GetTypeIndex(); +// +// template +// std::vector GetTypeIndices() { +// return {GetTypeIndex()...}; +// } +// }; +// template +// class Command { +// public: +// using BehaviorPtr = std::shared_ptr; +// +// template +// Command(Function&& func, std::string name) +// : func_{std::make_shared(std::forward(func))}, name_{std::move(name)} {} +// +// BehaviorPtr Execute() const { +// return func_(); +// } +// +// std::string GetName() const { +// return name_; +// } +// +// private: +// std::function> func_; +// std::string name_; +// }; + +class AutoCommands { + public: + // template + // Commands(CommandTypes&&... commands) + // : commands_{std::make_shared>(std::forward(commands))...} {} + + AutoCommands( + std::vector()>>> commands) + : commands_(std::move(commands)) {} + + std::shared_ptr Execute(std::string command) { + auto it = std::find_if(commands_.begin(), commands_.end(), + [&command](const auto& cmd) { return cmd.first == command; }); + + if (it != commands_.end()) { + return (it->second)(); + } else { + throw std::invalid_argument("Command not found"); + } + } + + // bool IsValid(std::string command) const { + // auto it = std::find_if(commands_.begin(), commands_.end(), [&command](const auto& cmd) { + // return cmd->f == command; + // }); + + // return it != commands_.end(); + // } + + std::shared_ptr Run(std::string commandString) { return Execute(commandString); } + + private: + std::vector()>>> commands_ = {}; +}; + +// template +// class ConcreteCommand : public CommandBase { +// public: +// using CommandType = Command; + +// ConcreteCommand(std::string name, typename CommandType::FunctionType func) +// : command_(std::move(name), std::move(func)) {} +// +// void Execute(std::vector args) const override { +// // Convert args to the expected types and call Execute +// if (args.size() == sizeof...(Args)) { +// command_.Execute(std::any_cast(args[0])...); +// } else { +// throw std::invalid_argument("Incorrect number of arguments for command"); +// } +// } +// +// bool IsValid(std::vector argTypes) const override { +// return command_.IsValid(argTypes); +// } +// +// std::string GetName() const override { +// return command_.GetName(); +// } +// +// private: +// Command command_; +// }; + +class PathWeaver { public: - Pathplanner(); + PathWeaver(); frc::Trajectory getTrajectory(std::string_view path); private: fs::path deploy_directory = frc::filesystem::GetDeployDirectory(); }; + +class FollowPath : public behaviour::Behaviour { + public: + FollowPath(drivetrain::SwerveDrive* swerve, std::string path, bool flip, frc::Pose2d offset); + + void OnTick(units::time::second_t dt) override; + + void CalcTimer(); + + void OnStart() override; + + private: + std::vector CheckPoints(std::vector points); + + units::second_t _time; + frc::Timer _timer; + + std::string _pathName; + std::shared_ptr _path; + std::vector _poses; + + drivetrain::SwerveDrive* _swerve; + + int _currentPose = 0; + + frc::Pose2d _offset; +}; + +class AutoBuilder { + public: + AutoBuilder(drivetrain::SwerveDrive* swerve, std::function shouldFlipPath, std::string autoRoutine, + AutoCommands commands); + + void Invert(); + void SetAuto(std::string path); + + std::shared_ptr GetAutoPath(); + std::shared_ptr GetAutoPath(std::string path); + + behaviour::Behaviour GetPath(std::string path); + + frc::Pose2d JSONPoseToPose2d(wpi::json j); + + private: + std::string _path; + bool _flip; + drivetrain::SwerveDrive* _swerve; + + AutoCommands _commandsList; + + wpi::json* _currentPath; + wpi::json* _startingPose; + wpi::json* _commands; + + std::shared_ptr pathplan; + + std::vector> commands; +}; + +class SwerveAutoBuilder { + public: + SwerveAutoBuilder(drivetrain::SwerveDrive* swerve, std::string name, AutoCommands commands); + + void SetAuto(std::string path); + std::shared_ptr GetAutoRoutine(); + std::shared_ptr GetAutoRoutine(std::string path); + + drivetrain::SwerveDrive* GetSwerve(); + + private: + AutoBuilder* _builder; + drivetrain::SwerveDrive* _swerve; + std::string _name; +}; + } // namespace utils } // namespace wom diff --git a/wombat/src/main/include/utils/Util.h b/wombat/src/main/include/utils/Util.h index 665c85ec..09448dca 100644 --- a/wombat/src/main/include/utils/Util.h +++ b/wombat/src/main/include/utils/Util.h @@ -28,39 +28,34 @@ units::second_t now(); class NTBound { public: - NTBound(std::shared_ptr table, std::string name, - const nt::Value& value, + NTBound(std::shared_ptr table, std::string name, const nt::Value& value, std::function onUpdateFn) - : _table(table), - _entry(table->GetEntry(name)), - _onUpdate(onUpdateFn), - _name(name) { + : _table(table), _entry(table->GetEntry(name)), _onUpdate(onUpdateFn), _name(name) { _entry.SetValue(value); // _listener = table->AddListener(name, , ([=](const nt::EntryNotification // &evt) { // this->_onUpdate(evt.value); // }, NT_NOTIFY_UPDATE); - _listener = table->AddListener( - name, nt::EventFlags::kValueAll, - ([this](nt::NetworkTable* table, std::string_view key, - const nt::Event& event) { - std::cout << "NT UPDATE" << std::endl; - this->_onUpdate(event.GetValueEventData()->value); - })); + _listener = + table->AddListener(name, nt::EventFlags::kValueAll, + ([this](nt::NetworkTable* table, std::string_view key, const nt::Event& event) { + #ifdef DEBUG + std::cout << "NT UPDATE" << std::endl; + #endif + this->_onUpdate(event.GetValueEventData()->value); + })); } NTBound(const NTBound& other) - : _table(other._table), - _entry(other._entry), - _onUpdate(other._onUpdate), - _name(other._name) { - _listener = _table->AddListener( - _name, nt::EventFlags::kValueAll, - ([this](nt::NetworkTable* table, std::string_view key, - const nt::Event& event) { - std::cout << "NT UPDATE" << std::endl; - this->_onUpdate(event.GetValueEventData()->value); - })); + : _table(other._table), _entry(other._entry), _onUpdate(other._onUpdate), _name(other._name) { + _listener = + _table->AddListener(_name, nt::EventFlags::kValueAll, + ([this](nt::NetworkTable* table, std::string_view key, const nt::Event& event) { + #ifdef DEBUG + std::cout << "NT UPDATE" << std::endl; + #endif + this->_onUpdate(event.GetValueEventData()->value); + })); } ~NTBound() { _table->RemoveListener(_listener); } @@ -75,29 +70,24 @@ class NTBound { class NTBoundDouble : public NTBound { public: - NTBoundDouble(std::shared_ptr table, std::string name, - double& val) + NTBoundDouble(std::shared_ptr table, std::string name, double& val) : NTBound(table, name, nt::Value::MakeDouble(val), [&val](const nt::Value& v) { val = v.GetDouble(); }) {} }; +// sussy baka should be constrained to a unit type template class NTBoundUnit : public NTBound { public: - NTBoundUnit(std::shared_ptr table, std::string name, - units::unit_t& val) + NTBoundUnit(std::shared_ptr table, std::string name, units::unit_t& val) : NTBound(table, name, nt::Value::MakeDouble(val.value()), - [&val](const nt::Value& v) { - val = units::unit_t{v.GetDouble()}; - }) {} + [&val](const nt::Value& v) { val = units::unit_t{v.GetDouble()}; }) {} }; void WritePose2NT(std::shared_ptr table, frc::Pose2d pose); void WritePose3NT(std::shared_ptr table, frc::Pose3d pose); -void WriteTrajectory(std::shared_ptr table, - frc::Trajectory trajectory); -void WriteTrajectoryState(std::shared_ptr table, - frc::Trajectory::State state); +void WriteTrajectory(std::shared_ptr table, frc::Trajectory trajectory); +void WriteTrajectoryState(std::shared_ptr table, frc::Trajectory::State state); frc::Pose2d TrajectoryStateToPose2d(frc::Trajectory::State state); diff --git a/wombat/src/main/include/vision/Camera.h b/wombat/src/main/include/vision/Camera.h new file mode 100644 index 00000000..83ce620d --- /dev/null +++ b/wombat/src/main/include/vision/Camera.h @@ -0,0 +1,68 @@ +// Copyright (c) 2023-2024 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + +#pragma once + +#include + +#include +#include +#include + +#include "frc/geometry/Transform2d.h" +#include "frc/geometry/Transform3d.h" +#include "photonlib/PhotonPipelineResult.h" +#include "photonlib/PhotonTrackedTarget.h" +#include "units/angle.h" +#include "wpi/SmallVector.h" + +namespace wom { +namespace vision { + +enum class PhotonVisionModes { kNormal = 0, kNotes = 1 }; + +using PhotonTarget = photonlib::PhotonTrackedTarget; + +using PhotonVisionLEDMode = photonlib::LEDMode; + +class PhotonVision { + public: + PhotonVision(); + explicit PhotonVision(std::string name); + + photonlib::PhotonPipelineResult GetResult(); + + void SetLED(PhotonVisionLEDMode mode); + void SetPipelineIndex(int index); + + bool HasTarget(); + std::vector GetTargets(); + PhotonTarget GetTarget(); + + double GetTargetYaw(PhotonTarget target); + double GetTargetPitch(PhotonTarget target); + double GetTargetArea(PhotonTarget target); + double GetTargetSkew(PhotonTarget target); + + frc::Transform3d GetCameraToTarget(PhotonTarget target); + std::vector> GetTargetCorners(PhotonTarget target); + + int GetTargetId(PhotonTarget target); + frc::Transform3d BestCameraToTarget(PhotonTarget target); + frc::Transform3d AlternateCameraToTarget(PhotonTarget target); + + PhotonVisionModes GetMode(); + void SetMode(PhotonVisionModes mode); + + private: + void Initialize(); + + std::string _name; + + PhotonVisionModes _mode = PhotonVisionModes::kNotes; + + photonlib::PhotonCamera* _camera; +}; +} // namespace vision +} // namespace wom diff --git a/wombat/src/main/include/vision/Limelight.h b/wombat/src/main/include/vision/Limelight.h index ddfb3739..4767a44b 100644 --- a/wombat/src/main/include/vision/Limelight.h +++ b/wombat/src/main/include/vision/Limelight.h @@ -26,20 +26,11 @@ namespace wom { namespace vision { -enum class LimelightLEDMode { - kPipelineDefault = 0, - kForceOff = 1, - kForceBlink = 2, - kForceOn = 3 -}; +enum class LimelightLEDMode { kPipelineDefault = 0, kForceOff = 1, kForceBlink = 2, kForceOn = 3 }; enum class LimelightCamMode { kVisionProcessor = 0, kDriverCamera = 1 }; -enum class LimelightStreamMode { - kStandard = 0, - kPiPMain = 1, - kPiPSecondary = 2 -}; +enum class LimelightStreamMode { kStandard = 0, kPiPMain = 1, kPiPSecondary = 2 }; enum class LimelightSnapshotMode { kReset = 0, kSingle = 1 }; @@ -71,6 +62,7 @@ enum class LimelightTargetingData { kJson = 11, kTclass = 12, kTc = 13, + kTid = 14 }; enum class LimelightAprilTagData { @@ -82,7 +74,6 @@ enum class LimelightAprilTagData { kTargetpose_robotspace = 5, kBotpose_targetspace = 6, kCamerapose_robotspace = 7, - kTid = 8 }; class Limelight { @@ -91,14 +82,12 @@ class Limelight { std::string GetName(); - std::shared_ptr table = - nt::NetworkTableInstance::GetDefault().GetTable("limelight"); + std::shared_ptr table = nt::NetworkTableInstance::GetDefault().GetTable("limelight"); std::pair GetOffset(); std::vector GetAprilTagData(LimelightAprilTagData dataType); - double GetTargetingData(LimelightTargetingData dataType, - double defaultValue = 0.0); + double GetTargetingData(LimelightTargetingData dataType, double defaultValue = 0.0); void SetLEDMode(LimelightLEDMode mode); void SetCamMode(LimelightCamMode mode); void SetPipeline(LimelightPipeline pipeline); @@ -110,12 +99,15 @@ class Limelight { void OnStart(); bool IsAtSetPoseVision(frc::Pose3d pose, units::second_t dt); + bool IsAtSetPoseVision(frc::Pose2d pose, units::second_t dt); - units::meters_per_second_t GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2, - units::second_t dt); + units::meters_per_second_t GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2, units::second_t dt); + units::meters_per_second_t GetSpeed(frc::Pose2d pose1, frc::Pose2d pose2, units::second_t dt); frc::Pose3d GetPose(); + bool HasTarget(); + private: std::string _limelightName; }; diff --git a/wombat/vendordeps/PathplannerLib.json b/wombat/vendordeps/PathplannerLib.json new file mode 100644 index 00000000..48b56188 --- /dev/null +++ b/wombat/vendordeps/PathplannerLib.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib.json", + "name": "PathplannerLib", + "version": "2024.2.3", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2024", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2024.2.3" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2024.2.3", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} diff --git a/wombat/vendordeps/WPILibNewCommands.json b/wombat/vendordeps/WPILibNewCommands.json new file mode 100644 index 00000000..67bf3898 --- /dev/null +++ b/wombat/vendordeps/WPILibNewCommands.json @@ -0,0 +1,38 @@ +{ + "fileName": "WPILibNewCommands.json", + "name": "WPILib-New-Commands", + "version": "1.0.0", + "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "frcYear": "2024", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-cpp", + "version": "wpilib", + "libName": "wpilibNewCommands", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxuniversal" + ] + } + ] +} diff --git a/wombat/vendordeps/photonlib.json b/wombat/vendordeps/photonlib.json new file mode 100644 index 00000000..6a4780c7 --- /dev/null +++ b/wombat/vendordeps/photonlib.json @@ -0,0 +1,42 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2024.1.1-beta-3.2", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2024", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-cpp", + "version": "v2024.1.1-beta-3.2", + "libName": "Photon", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-java", + "version": "v2024.1.1-beta-3.2" + }, + { + "groupId": "org.photonvision", + "artifactId": "PhotonTargeting-java", + "version": "v2024.1.1-beta-3.2" + } + ] +}