subsystem frequency modification

Team846 · Jan 25, 2025 · a606b72 · a606b72
1 parent 447785e
commit a606b72
Show file tree

Hide file tree

Showing 8 changed files with 246 additions and 24 deletions.
diff --git 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]
 ```
diff --git 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
@@ -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
@@ -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
@@ -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<units::millisecond>(), 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
@@ -8,20 +8,66 @@ class GenericRobotContainer : public frc846::base::Loggable {
 public:
   GenericRobotContainer() : frc846::base::Loggable{"robot_container"} {}
 
-  void RegisterSubsystems(
-      std::vector<frc846::robot::SubsystemBase*> subsystems) {
-    all_subsystems_ = subsystems;
+  void RegisterSubsystemGroupA(
+      std::initializer_list<std::pair<frc846::robot::SubsystemBase*, bool>>
+          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<std::pair<frc846::robot::SubsystemBase*, bool>>
+          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<std::pair<frc846::robot::SubsystemBase*, bool>>
+          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<int>(loop.to<double>()) % 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<int>(loop.to<double>()) % 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<frc846::robot::SubsystemBase*> all_subsystems_{};
+  std::vector<frc846::robot::SubsystemBase*> group_a_subsystems_{};
+  std::vector<frc846::robot::SubsystemBase*> group_b_subsystems_{};
 };
 
 }  // namespace frc846::robot
diff --git 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
@@ -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}});
   }
 };