diff --git a/README.md b/README.md index 7d80864..967dd78 100644 --- a/README.md +++ b/README.md @@ -173,6 +173,6 @@ To undo the going back: ## CppCheck Warnings ``` -src\frc846\cpp\frc846\math\collection.cc:25:0: warning: The function 'VerticalDeadband' is never used. [unusedFunction] -src\frc846\cpp\frc846\math\collection.cc:52:0: warning: The function 'CoterminalSum' is never used. [unusedFunction] +src/frc846/cpp/frc846/math/collection.cc:25:0: warning: The function 'VerticalDeadband' is never used. [unusedFunction] +src/frc846/cpp/frc846/math/collection.cc:52:0: warning: The function 'CoterminalSum' is never used. [unusedFunction] ``` \ No newline at end of file diff --git a/build.gradle b/build.gradle index 91dd9fd..1baf879 100644 --- a/build.gradle +++ b/build.gradle @@ -37,7 +37,7 @@ deploy { def deployArtifact = deploy.targets.roborio.artifacts.frcCpp // Set this to true to enable desktop support. -def includeDesktopSupport = false +def includeDesktopSupport = true // Set to true to run simulation in debug mode wpi.cpp.debugSimulation = true diff --git a/networktables.json b/networktables.json index c2d87d1..d742fb5 100644 --- a/networktables.json +++ b/networktables.json @@ -438,5 +438,85 @@ "properties": { "persistent": true } + }, + { + "name": "/Preferences/SwerveDrivetrain/bearing_gains/_kP", + "type": "double", + "value": 0.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SwerveDrivetrain/bearing_gains/_kI", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SwerveDrivetrain/bearing_gains/_kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SwerveDrivetrain/bearing_gains/deadband (deg_per_s)", + "type": "double", + "value": 3.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SwerveDrivetrain/lock_gains/_kP", + "type": "double", + "value": 0.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SwerveDrivetrain/lock_gains/_kI", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SwerveDrivetrain/lock_gains/_kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SwerveDrivetrain/lock_gains/deadband (in)", + "type": "double", + "value": 2.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SwerveDrivetrain/drive_to_subtract (in)", + "type": "double", + "value": 5.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SwerveDrivetrain/max_omega_cut (deg_per_s)", + "type": "double", + "value": 40.0, + "properties": { + "persistent": true + } } ] diff --git a/networktables.json.bck b/networktables.json.bck index 24e7511..c2d87d1 100644 --- a/networktables.json.bck +++ b/networktables.json.bck @@ -350,5 +350,93 @@ "properties": { "persistent": true } + }, + { + "name": "/Preferences/SwerveDrivetrain/BR/cancoder_offset_ (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SwerveDrivetrain/max_speed (fps)", + "type": "double", + "value": 15.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SwerveDrivetrain/max_omega (deg_per_s)", + "type": "double", + "value": 180.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SwerveDrivetrain/steer_lag (s)", + "type": "double", + "value": 0.05, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/MotorMonkey/voltage_min (V)", + "type": "double", + "value": 8.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/MotorMonkey/recal_voltage_thresh (V)", + "type": "double", + "value": 10.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/MotorMonkey/default_max_draw (A)", + "type": "double", + "value": 150.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/MotorMonkey/min_max_draw (A)", + "type": "double", + "value": 60.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/MotorMonkey/max_max_draw (A)", + "type": "double", + "value": 250.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/MotorMonkey/battery_cc (A)", + "type": "double", + "value": 700.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/MotorMonkey/brownout_perm_loops", + "type": "int", + "value": 500, + "properties": { + "persistent": true + } } ] diff --git a/src/frc846/cpp/frc846/robot/GenericRobot.cc b/src/frc846/cpp/frc846/robot/GenericRobot.cc index 95e96fe..9917b32 100644 --- a/src/frc846/cpp/frc846/robot/GenericRobot.cc +++ b/src/frc846/cpp/frc846/robot/GenericRobot.cc @@ -168,14 +168,7 @@ void GenericRobot::StartCompetition() { OnPeriodic(); - // Update subsystem readings - generic_robot_container_->UpdateReadings(); - - // Tick command scheduler - frc2::CommandScheduler::GetInstance().Run(); - - // Update subsystem hardware - generic_robot_container_->UpdateHardware(); + // moved this stuff down // Tick MotorMonkey frc846::control::MotorMonkey::Tick(mode == Mode::kDisabled); @@ -211,6 +204,15 @@ void GenericRobot::StartCompetition() { Warn("Loop overrun: {} (loop period: {})", loop_time.convert(), kPeriod); } + + // Update subsystem readings + generic_robot_container_->UpdateReadings(loop_time); + + // Tick command scheduler + frc2::CommandScheduler::GetInstance().Run(); + + // Update subsystem hardware + generic_robot_container_->UpdateHardware(loop_time); } } diff --git a/src/frc846/include/frc846/robot/GenericRobotContainer.h b/src/frc846/include/frc846/robot/GenericRobotContainer.h index e5b22d9..abf87e0 100644 --- a/src/frc846/include/frc846/robot/GenericRobotContainer.h +++ b/src/frc846/include/frc846/robot/GenericRobotContainer.h @@ -8,20 +8,66 @@ class GenericRobotContainer : public frc846::base::Loggable { public: GenericRobotContainer() : frc846::base::Loggable{"robot_container"} {} - void RegisterSubsystems( - std::vector subsystems) { - all_subsystems_ = subsystems; + void RegisterSubsystemGroupA( + std::initializer_list> + subsystems) { + for (auto& [subsystem, init] : subsystems) { + if (init) { + subsystem->Init(); + group_a_subsystems_.push_back(subsystem); + all_subsystems_.push_back(subsystem); + } + } } - void UpdateReadings() { - for (auto subsystem : all_subsystems_) { - subsystem->UpdateReadings(); + void RegisterSubsystemGroupB( + std::initializer_list> + subsystems) { + for (auto& [subsystem, init] : subsystems) { + if (init) { + subsystem->Init(); + group_b_subsystems_.push_back(subsystem); + all_subsystems_.push_back(subsystem); + } } } - void UpdateHardware() { - for (auto subsystem : all_subsystems_) { - subsystem->UpdateHardware(); + void RegisterSubsystemGroupAB( + std::initializer_list> + subsystems) { + for (auto& [subsystem, init] : subsystems) { + if (init) { + subsystem->Init(); + group_a_subsystems_.push_back(subsystem); + group_b_subsystems_.push_back(subsystem); + all_subsystems_.push_back(subsystem); + } + } + } + + void UpdateReadings(units::time::second_t loop) { + int calc_loop = static_cast(loop.to()) % 2; + if (calc_loop % 2 == 0) { + for (auto subsystem : group_a_subsystems_) { + subsystem->UpdateReadings(); + } + } else { + for (auto subsystem : group_b_subsystems_) { + subsystem->UpdateReadings(); + } + } + } + + void UpdateHardware(units::time::second_t loop) { + int calc_loop = static_cast(loop.to()) % 2; + if (calc_loop % 2 == 0) { + for (auto subsystem : group_a_subsystems_) { + subsystem->UpdateHardware(); + } + } else { + for (auto subsystem : group_b_subsystems_) { + subsystem->UpdateHardware(); + } } } @@ -45,6 +91,8 @@ class GenericRobotContainer : public frc846::base::Loggable { private: std::vector all_subsystems_{}; + std::vector group_a_subsystems_{}; + std::vector group_b_subsystems_{}; }; } // namespace frc846::robot diff --git a/src/frc846/include/frc846/robot/GenericSubsystem.h b/src/frc846/include/frc846/robot/GenericSubsystem.h index 302cb14..8d617c6 100644 --- a/src/frc846/include/frc846/robot/GenericSubsystem.h +++ b/src/frc846/include/frc846/robot/GenericSubsystem.h @@ -21,6 +21,8 @@ class SubsystemBase : public frc846::base::Loggable { virtual ~SubsystemBase() = default; + virtual void Init() = 0; + virtual void Setup() = 0; virtual void UpdateReadings() = 0; @@ -49,7 +51,7 @@ class GenericSubsystem : public frc2::SubsystemBase, public SubsystemBase { virtual ~GenericSubsystem() { Warn("Destroying subsystem"); }; // Initializer function for RobotContainer use only. - void Init() { + void Init() override final { SetName(name()); Log("Initializing subsystem"); init_ = true; diff --git a/src/y2025/include/subsystems/robot_container.h b/src/y2025/include/subsystems/robot_container.h index 5a96742..579fbe7 100644 --- a/src/y2025/include/subsystems/robot_container.h +++ b/src/y2025/include/subsystems/robot_container.h @@ -19,9 +19,11 @@ class RobotContainer : public frc846::robot::GenericRobotContainer { RegisterPreference("init_leds", true); control_input_.Init(); - if (GetPreferenceValue_bool("init_drivetrain")) drivetrain_.Init(); - if (GetPreferenceValue_bool("init_leds")) leds_.Init(); + bool drivetrain_init = (GetPreferenceValue_bool("init_drivetrain")); + bool leds_init = (GetPreferenceValue_bool("init_leds")); - RegisterSubsystems({&control_input_, &drivetrain_, &leds_}); + RegisterSubsystemGroupA({{&control_input_, true}}); + RegisterSubsystemGroupB({{&leds_, leds_init}}); + RegisterSubsystemGroupAB({{&drivetrain_, drivetrain_init}}); } };