got rid of custom loop time

src/main/cpp/str/SwerveDrive.cpp

@@ -72,10 +72,6 @@ void SwerveDrive::Drive(units::meters_per_second_t xVel,
                         units::meters_per_second_t yVel,
                         units::radians_per_second_t omega, bool fieldRelative,
                         bool openLoop) {
-  units::second_t now = frc::Timer::GetFPGATimestamp();
-  units::second_t loopTime = now - lastDriveLoopTime;
-  loopTimePub.Set((1 / loopTime).value());
-
   frc::ChassisSpeeds speedsToSend;
   if (fieldRelative) {
     speedsToSend = frc::ChassisSpeeds::FromFieldRelativeSpeeds(
@@ -86,14 +82,12 @@ void SwerveDrive::Drive(units::meters_per_second_t xVel,
     speedsToSend.omega = omega;
   }
 
-  speedsToSend = frc::ChassisSpeeds::Discretize(speedsToSend, loopTime);
+  speedsToSend = frc::ChassisSpeeds::Discretize(speedsToSend, consts::LOOP_PERIOD);
 
   SetModuleStates(
       consts::swerve::physical::KINEMATICS.ToSwerveModuleStates(speedsToSend),
       true, openLoop,
       ConvertModuleForcesToTorqueCurrent(xModuleForce, yModuleForce));
-
-  lastDriveLoopTime = now;
 }
 
 frc::ChassisSpeeds SwerveDrive::GetRobotRelativeSpeeds() const {
@@ -190,27 +184,22 @@ void SwerveDrive::UpdateNTEntries() {
 }
 
 void SwerveDrive::SimulationPeriodic() {
-  units::second_t now = frc::Timer::GetFPGATimestamp();
-  units::second_t loopTime = now - lastSimLoopTime;
-
   std::array<frc::SwerveModuleState, 4> simState;
   int i = 0;
   for (auto &swerveModule : modules) {
     simState[i] = swerveModule.UpdateSimulation(
-        loopTime, frc::RobotController::GetBatteryVoltage());
+        consts::LOOP_PERIOD, frc::RobotController::GetBatteryVoltage());
     i++;
   }
 
   simStatesPub.Set(simState);
 
   units::radians_per_second_t omega =
       consts::swerve::physical::KINEMATICS.ToChassisSpeeds(simState).omega;
-  units::radian_t angleChange = omega * loopTime;
+  units::radian_t angleChange = omega * consts::LOOP_PERIOD;
 
   lastSimAngle = lastSimAngle + frc::Rotation2d{angleChange};
   imuSimState.SetRawYaw(lastSimAngle.Degrees());
-
-  lastSimLoopTime = now;
 }
 
 units::ampere_t SwerveDrive::GetSimulatedCurrentDraw() const {

src/main/cpp/subsystems/IntakeSubsystem.cpp

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "subsystems/IntakeSubsystem.h"
+#include <frc/smartdashboard/SmartDashboard.h>
+
+IntakeSubsystem::IntakeSubsystem() {
+
+    ConfigureIntakeMotor(consts::intake::physical::INVERT_MOTOR, consts::intake::physical::INTAKE_RATIO, consts::intake::current_limits::SUPPLY_CURRENT_LIMIT, consts::intake::current_limits::STATOR_CURRENT_LIMIT);
+    ConfigureMotorSignals();
+    SetName("IntakeSubsystem");
+    frc::SmartDashboard::PutData(this);
+}
+
+// This method will be called once per scheduler run
+void IntakeSubsystem::Periodic() {}
+
+bool IntakeSubsystem::ConfigureIntakeMotor(bool invert,
+                            units::scalar_t intakeGearing,
+                            units::ampere_t supplyCurrentLimit,
+                            units::ampere_t statorCurrentLimit) {
+                                return true;
+                            }
+
+
+bool IntakeSubsystem::ConfigureMotorSignals() {
+    return true;
+}

src/main/cpp/subsystems/ShooterSubsystem.cpp

@@ -5,6 +5,8 @@
 #include "subsystems/ShooterSubsystem.h"
 #include "constants/Constants.h"
 #include <frc2/command/Commands.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
 
 ShooterSubsystem::ShooterSubsystem() {
   ConfigureShooterMotors(consts::shooter::physical::BOTTOM_INVERT,
@@ -13,6 +15,8 @@ ShooterSubsystem::ShooterSubsystem() {
                          consts::shooter::current_limits::SUPPLY_CURRENT_LIMIT,
                          consts::shooter::current_limits::STATOR_CURRENT_LIMIT);
   ConfigureMotorSignals();
+  SetName("ShooterSubsystem");
+  frc::SmartDashboard::PutData(this);
 }
 
 frc2::CommandPtr ShooterSubsystem::RunShooter(std::function<consts::shooter::PRESET_SPEEDS()> preset) {

src/main/deploy/commit.txt

@@ -1 +1 @@
-d7b9781
+f5a5c55

src/main/include/constants/IntakeConstants.h

@@ -0,0 +1,41 @@
+// Copyright (c) FRC 2053.
+// Open Source Software; you can modify and/or share it under the terms of
+// the MIT License file in the root of this project
+
+#pragma once
+
+#include <frc/system/plant/DCMotor.h>
+#include <units/base.h>
+#include <units/velocity.h>
+#include <wpi/interpolating_map.h>
+#include <units/dimensionless.h>
+#include <units/moment_of_inertia.h>
+
+#include "str/Gains.h"
+
+namespace consts {
+namespace intake {
+namespace can_ids {
+inline constexpr int INTAKE = 17;
+} // namespace can_ids
+
+namespace current_limits {
+inline constexpr units::ampere_t SUPPLY_CURRENT_LIMIT = 60_A;
+inline constexpr units::ampere_t STATOR_CURRENT_LIMIT = 180_A;
+} // namespace current_limits
+
+namespace physical {
+
+inline constexpr bool INVERT_MOTOR = false;
+
+inline constexpr units::scalar_t INTAKE_RATIO = (24.0 / 16.0);
+
+inline constexpr frc::DCMotor SHOOTER_MOTOR = frc::DCMotor::Falcon500(1);
+
+inline constexpr units::meter_t WHEEL_RADIUS = 1_in;
+
+//From onshape doc
+inline constexpr units::kilogram_square_meter_t FLYWHEEL_MOI = 12.350445 * 1_in * 1_in * 1_lb;
+} // namespace physical
+} // namespace intake
+} // namespace consts

src/main/include/str/SwerveDrive.h

@@ -152,17 +152,10 @@ class SwerveDrive {
       modulePositions, frc::Pose2d{}};
 
   units::radian_t yawLatencyComped;
-  units::second_t lastDriveLoopTime;
-  units::second_t lastSimLoopTime;
   frc::Rotation2d lastSimAngle;
 
   std::shared_ptr<nt::NetworkTable> nt{
       nt::NetworkTableInstance::GetDefault().GetTable("SwerveDrive")};
-  nt::DoublePublisher loopTimePub{
-      nt::NetworkTableInstance::GetDefault()
-          .GetTable("Metadata")
-          ->GetDoubleTopic("SwerveDriveOdomLoopRate")
-          .Publish()};
   nt::BooleanPublisher isSlippingPub{
       nt->GetBooleanTopic("IsSlipping").Publish()};
   nt::StructArrayPublisher<frc::SwerveModuleState> desiredStatesPub{

src/main/include/subsystems/IntakeSubsystem.h

@@ -0,0 +1,34 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc2/command/SubsystemBase.h>
+#include <units/current.h>
+#include <units/base.h>
+#include <networktables/NetworkTableInstance.h>
+#include <networktables/DoubleTopic.h>
+#include <constants/IntakeConstants.h>
+
+class IntakeSubsystem : public frc2::SubsystemBase {
+ public:
+  IntakeSubsystem();
+
+  /**
+   * Will be called periodically whenever the CommandScheduler runs.
+   */
+  void Periodic() override;
+
+
+ private:
+  void UpdateNTEntries();
+  bool ConfigureIntakeMotor(bool invert,
+                              units::scalar_t intakeGearing,
+                              units::ampere_t supplyCurrentLimit,
+                              units::ampere_t statorCurrentLimit);
+  bool ConfigureMotorSignals();
+
+  std::shared_ptr<nt::NetworkTable> nt{
+      nt::NetworkTableInstance::GetDefault().GetTable("Intake")};
+};

src/main/include/subsystems/ShooterSubsystem.h

@@ -126,11 +126,6 @@ class ShooterSubsystem : public frc2::SubsystemBase {
 
   std::shared_ptr<nt::NetworkTable> nt{
       nt::NetworkTableInstance::GetDefault().GetTable("Shooter")};
-  nt::DoublePublisher loopTimePub{
-      nt::NetworkTableInstance::GetDefault()
-          .GetTable("Metadata")
-          ->GetDoubleTopic("ShooterOdomLoopRate")
-          .Publish()};
   nt::DoublePublisher topWheelVelocityPub{
     nt->GetDoubleTopic("TopWheelVelocityRPM").Publish()
   };