Characterization (#176)

* Fix characterization * Ad newline in SysIdDynamic function definition Co-authored-by: Evan Rust <[email protected]> * Remove semicolon in function definition Co-authored-by: Evan Rust <[email protected]> * Add newlines to characterization constants Co-authored-by: Evan Rust <[email protected]> * Remove unnecessary comments --------- Co-authored-by: 5690 <[email protected]> Co-authored-by: Evan Rust <[email protected]>
SubZero-Robotics · Jul 10, 2024 · ca73733 · ca73733
1 parent 16c9abd
commit ca73733
Show file tree

Hide file tree

Showing 8 changed files with 144 additions and 4 deletions.
diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp
@@ -139,7 +139,7 @@ void RobotContainer::ConfigureButtonBindings() {
               [this] { return m_driverController.GetRightTriggerAxis(); },
               [this] { return 0; })
               .ToPtr());
-
+#ifndef USING_SYSID
   m_driverController.B().OnTrue(
       m_leds.Intaking()
           .AndThen(m_leds.AngryFace())
@@ -171,6 +171,19 @@ void RobotContainer::ConfigureButtonBindings() {
                                      &m_scoring, &m_intake, &m_arm))
           .AndThen(m_leds.BlinkingFace())
           .AndThen(m_leds.Idling()));
+#elif USING_SYSID
+  m_driverController.A().OnTrue(
+      m_drive.SysIdQuasistatic(frc2::sysid::Direction::kForward));
+
+  m_driverController.B().OnTrue(
+      m_drive.SysIdQuasistatic(frc2::sysid::Direction::kReverse));
+
+  m_driverController.X().OnTrue(
+      m_drive.SysIdDynamic(frc2::sysid::Direction::kForward));
+
+  m_driverController.Y().OnTrue(
+      m_drive.SysIdDynamic(frc2::sysid::Direction::kReverse));
+#endif
 
   m_driverController.LeftBumper()
       .OnTrue(m_leds.Climbing().AndThen(m_leds.AmogusFace()))

diff --git a/src/main/cpp/subsystems/DriveSubsystem.cpp b/src/main/cpp/subsystems/DriveSubsystem.cpp
@@ -61,6 +61,11 @@ DriveSubsystem::DriveSubsystem(PhotonVisionEstimators* vision)
       },
       this);
 
+  m_driveModules = {&m_frontLeft, &m_rearLeft, &m_frontRight, &m_rearRight};
+
+  m_sysIdRoutine =
+      makeSysIdRoutine(kMotorNames, m_driveModules, MotorType::DriveMotor);
+
   m_publisher =
       nt::NetworkTableInstance::GetDefault()
           .GetStructArrayTopic<frc::SwerveModuleState>("/SwerveStates")
@@ -71,6 +76,33 @@ DriveSubsystem::DriveSubsystem(PhotonVisionEstimators* vision)
           .Publish();
 }
 
+std::unique_ptr<frc2::sysid::SysIdRoutine> DriveSubsystem::makeSysIdRoutine(
+    std::vector<std::string> motorNames, std::vector<MAXSwerveModule*> modules,
+    MotorType motorType) {
+  return std::make_unique<frc2::sysid::SysIdRoutine>(
+      frc2::sysid::Config{std::nullopt, std::nullopt, std::nullopt, nullptr},
+      frc2::sysid::Mechanism{
+          [this, motorType](units::volt_t driveVoltage) {
+            for (auto* module : m_driveModules) {
+              module->SetMotorVoltage(motorType, driveVoltage);
+            }
+          },
+          [this, motorNames, motorType](frc::sysid::SysIdRoutineLog* log) {
+            ConsoleWriter.logVerbose("LOG WRITE", "log write called%s", "");
+            for (size_t i = 0; i < motorNames.size(); i++) {
+              log->Motor(motorNames[i])
+                  .voltage(
+                      units::volt_t{m_driveModules[i]->Get(motorType) *
+                                    frc::RobotController::GetBatteryVoltage()})
+                  .position(
+                      units::meter_t{m_driveModules[i]->GetDistance(motorType)})
+                  .velocity(units::meters_per_second_t{
+                      m_driveModules[i]->GetRate(motorType)});
+            }
+          },
+          this});
+}
+
 void DriveSubsystem::SimulationPeriodic() {
   logDrivebase();
 
@@ -104,6 +136,16 @@ void DriveSubsystem::Periodic() {
   }
 }
 
+frc2::CommandPtr DriveSubsystem::SysIdQuasistatic(
+    frc2::sysid::Direction direction) {
+  return m_sysIdRoutine->Quasistatic(direction);
+}
+
+frc2::CommandPtr DriveSubsystem::SysIdDynamic(
+    frc2::sysid::Direction direction) {
+  return m_sysIdRoutine->Dynamic(direction);
+}
+
 void DriveSubsystem::Drive(units::meters_per_second_t xSpeed,
                            units::meters_per_second_t ySpeed,
                            units::radians_per_second_t rot, bool fieldRelative,
@@ -194,10 +236,11 @@ void DriveSubsystem::logDrivebase() {
                             m_rearLeft.GetState(), m_rearRight.GetState()};
   std::span<frc::SwerveModuleState, 4> states(states_vec.begin(),
                                               states_vec.end());
-  std::vector desired_vec = {m_frontLeft.GetDesiredState(), m_frontRight.GetDesiredState(),
-                            m_rearLeft.GetDesiredState(), m_rearRight.GetDesiredState()};
+  std::vector desired_vec = {
+      m_frontLeft.GetDesiredState(), m_frontRight.GetDesiredState(),
+      m_rearLeft.GetDesiredState(), m_rearRight.GetDesiredState()};
   std::span<frc::SwerveModuleState, 4> desiredStates(desired_vec.begin(),
-                                              desired_vec.end());
+                                                     desired_vec.end());
 
   m_publisher.Set(states);
   m_desiredPublisher.Set(desiredStates);

diff --git a/src/main/cpp/subsystems/MAXSwerveModule.cpp b/src/main/cpp/subsystems/MAXSwerveModule.cpp
@@ -162,6 +162,35 @@ void MAXSwerveModule::SetDesiredState(
   }
 }
 
+void MAXSwerveModule::SetMotorVoltage(MotorType type, units::volt_t voltage) {
+  if (type == MotorType::DriveMotor) {
+    m_drivingSparkMax.SetVoltage(voltage);
+    return;
+  }
+  m_turningSparkMax.SetVoltage(voltage);
+}
+
+double MAXSwerveModule::Get(MotorType type) {
+  if (type == MotorType::DriveMotor) {
+    return m_drivingSparkMax.Get();
+  }
+  return m_turningSparkMax.Get();
+}
+
+double MAXSwerveModule::GetDistance(MotorType type) {
+  if (type == MotorType::DriveMotor) {
+    return m_drivingEncoder.GetPosition();
+  }
+  return m_turningAbsoluteEncoder.GetPosition();
+}
+
+double MAXSwerveModule::GetRate(MotorType type) {
+  if (type == MotorType::DriveMotor) {
+    return m_drivingEncoder.GetVelocity();
+  }
+  return m_turningAbsoluteEncoder.GetVelocity();
+}
+
 void MAXSwerveModule::simUpdateDrivePosition(
     const frc::SwerveModuleState& desiredState) {
   m_simDriveEncoderVelocity = desiredState.speed;

diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h
@@ -4,6 +4,8 @@
 
 #pragma once
 
+// #define USING_SYSID
+
 // Don't define as TEST_SWERVE_BOT if not using the testing swerve robot
 // #define TEST_SWERVE_BOT
 
@@ -23,6 +25,7 @@
 #include "constants/AutoConstants.h"
 #include "constants/BaseSingleAxisSubsystemConstants.h"
 #include "constants/CANConstants.h"
+#include "constants/CharacterizationConstants.h"
 #include "constants/ClimbConstants.h"
 #include "constants/DriveConstants.h"
 #include "constants/IntakingConstants.h"

diff --git a/src/main/include/constants/CharacterizationConstants.h b/src/main/include/constants/CharacterizationConstants.h
@@ -0,0 +1,27 @@
+#pragma once
+
+namespace CharacterizationConstants {
+namespace FrontLeftDrive {
+constexpr double kS = 0.43779;
+constexpr double kV = 2.4439;
+constexpr double kA = 0.36441;
+}  // namespace FrontLeftDrive
+
+namespace FrontRightDrive {
+constexpr double kS = 0.47478;
+constexpr double kV = 2.4511;
+constexpr double kA = 0.36441;
+}  // namespace FrontRightDrive
+
+namespace RearLeftDrive {
+constexpr double kS = 0.43972;
+constexpr double kV = 2.4352;
+constexpr double kA = 0.35057;
+}  // namespace RearLeftDrive
+
+namespace RearRightDrive {
+constexpr double kS = 0.41185;
+constexpr double kV = 2.4218;
+constexpr double kA = 0.41332;
+}  // namespace RearRightDrive
+}  // namespace CharacterizationConstants
diff --git a/src/main/include/constants/DriveConstants.h b/src/main/include/constants/DriveConstants.h
@@ -47,4 +47,8 @@ constexpr int kRearLeftTurningCanId = 5;
 
 constexpr units::second_t kLoopTime = 0.022_s;
 constexpr double kLoopsPerSecond = 1_s / kLoopTime;
+
+const std::vector<std::string> kMotorNames = {
+    "Front-Left-Drive", "Rear-Left-Drive", "Front-Right-Drive",
+    "Rear-Right-Drive"};
 }  // namespace DriveConstants
diff --git a/src/main/include/subsystems/DriveSubsystem.h b/src/main/include/subsystems/DriveSubsystem.h
@@ -14,6 +14,7 @@
 #include <frc/kinematics/SwerveDriveOdometry.h>
 #include <frc/smartdashboard/Field2d.h>
 #include <frc2/command/SubsystemBase.h>
+#include <frc2/command/sysid/SysIdRoutine.h>
 #include <hal/SimDevice.h>
 #include <hal/simulation/SimDeviceData.h>
 #include <networktables/StructArrayTopic.h>
@@ -156,6 +157,9 @@ class DriveSubsystem : public frc2::SubsystemBase {
       frc::Translation2d{-DriveConstants::kWheelBase / 2,
                          -DriveConstants::kTrackWidth / 2}};
 
+  frc2::CommandPtr SysIdQuasistatic(frc2::sysid::Direction direction);
+  frc2::CommandPtr SysIdDynamic(frc2::sysid::Direction direction);
+
  private:
   // Components (e.g. motor controllers and sensors) should generally be
   // declared private and exposed only through public methods.
@@ -167,6 +171,13 @@ class DriveSubsystem : public frc2::SubsystemBase {
                                            units::radians_per_second_t rot,
                                            bool fieldRelative);
 
+  std::unique_ptr<frc2::sysid::SysIdRoutine> makeSysIdRoutine(
+      std::vector<std::string> names, std::vector<MAXSwerveModule*> modules,
+      MotorType motorType);
+
+  std::vector<MAXSwerveModule*> m_driveModules;
+  std::unique_ptr<frc2::sysid::SysIdRoutine> m_sysIdRoutine;
+
   MAXSwerveModule m_frontLeft;
   MAXSwerveModule m_rearLeft;
   MAXSwerveModule m_frontRight;

diff --git a/src/main/include/subsystems/MAXSwerveModule.h b/src/main/include/subsystems/MAXSwerveModule.h
@@ -15,6 +15,8 @@
 
 #include "Constants.h"
 
+enum class MotorType { DriveMotor = 0, TurnMotor };
+
 class MAXSwerveModule {
  public:
   /**
@@ -62,6 +64,14 @@ class MAXSwerveModule {
    */
   void ResetEncoders();
 
+  void SetMotorVoltage(MotorType type, units::volt_t voltage);
+
+  double Get(MotorType type);
+
+  double GetDistance(MotorType type);
+
+  double GetRate(MotorType type);
+
  private:
   rev::CANSparkMax m_drivingSparkMax;
   rev::CANSparkMax m_turningSparkMax;