Implemented specific functions of DrivetrainSubsystem, added SwerveOd…

…ometryCalculator

src/frc846/cpp/frc846/robot/swerve/drivetrain.cc

@@ -2,12 +2,41 @@
 
 namespace frc846::robot::swerve {
 
-DrivetrainSubsystem::DrivetrainSubsystem(DrivetrainConfigs configs, bool enable)
+DrivetrainSubsystem::DrivetrainSubsystem(DrivetrainConfigs configs)
     : GenericSubsystem{"SwerveDrivetrain"}, configs_{configs}, modules_{} {
+  if (configs_.navX_connection_mode == NavX_connection_type::kSPI) {
+    navX_ = new AHRS{frc::SPI::Port::kMXP};
+  } else if (configs_.navX_connection_mode == NavX_connection_type::kSerial) {
+    navX_ = new AHRS{frc::SerialPort::Port::kMXP};
+  } else {
+    throw std::runtime_error("Invalid navX connection mode");
+  }
+
   // TODO: finish
+
+  for (int i = 0; i < 4; i++) {
+    // modules_[i] =
+    //     new SwerveModuleSubsystem{*this, "Module" + std::to_string(i)};
+  }
+
+  RegisterPreference("steer_gains/kP", 0.3);
+  RegisterPreference("steer_gains/kI", 0.0);
+  RegisterPreference("steer_gains/kD", 0.0);
+  RegisterPreference("steer_gains/kF", 0.0);
 }
 
 void DrivetrainSubsystem::Setup() {
+  frc846::control::base::MotorGains steer_gains{
+      GetPreferenceValue_double("steer_gains/kP"),
+      GetPreferenceValue_double("steer_gains/kI"),
+      GetPreferenceValue_double("steer_gains/kD"),
+      GetPreferenceValue_double("steer_gains/kF")};
+  for (SwerveModuleSubsystem* module : modules_) {
+    module->InitByParent();
+    module->Setup();
+    module->SetSteerGains(steer_gains);
+    module->ZeroWithCANcoder();
+  }
   // TODO: finish
 }
 
@@ -17,14 +46,43 @@ DrivetrainTarget DrivetrainSubsystem::ZeroTarget() const {
 
 bool DrivetrainSubsystem::VerifyHardware() {
   bool ok = true;
-  // TODO: finish
+  for (SwerveModuleSubsystem* module : modules_) {
+    ok &= module->VerifyHardware();
+  }
+  FRC846_VERIFY(ok, ok, "At least one module failed verification");
   return ok;
 }
 
 DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() {
-  DrivetrainReadings readings;
-  // TODO: finish
-  return readings;
+  units::degree_t bearing = navX_->GetAngle() * 1_deg;
+
+  frc846::math::VectorND<units::inch, 4> drive_positions{
+      0_in, 0_in, 0_in, 0_in};
+  std::array<units::degree_t, 4> steer_positions{};
+
+  frc846::math::VectorND<units::feet_per_second, 2> velocity{0_fps, 0_fps};
+
+  for (int i = 0; i < 4; i++) {
+    modules_[i]->UpdateReadings();
+    SwerveModuleReadings r = modules_[i]->GetReadings();
+
+    drive_positions[i] = r.drive_pos;
+    steer_positions[i] = r.steer_pos;
+
+    velocity += (frc846::math::VectorND<units::feet_per_second, 2>{
+        r.drive_pos, r.steer_pos + bearing, true});
+  }
+
+  velocity /= 4.0;
+
+  frc846::robot::swerve::odometry::SwervePose new_pose{
+      .position =
+          odometry_.calculate({bearing, steer_positions, drive_positions})
+              .position,
+      .bearing = bearing,
+      .velocity = velocity};
+
+  return {new_pose};
 }
 
 void DrivetrainSubsystem::WriteToHardware(DrivetrainTarget target) {

src/frc846/cpp/frc846/robot/swerve/odometry/swerve_odometry_calculator.cc

@@ -0,0 +1,25 @@
+#include "frc846/robot/swerve/odometry/swerve_odometry_calculator.h"
+
+namespace frc846::robot::swerve::odometry {
+
+SwerveOdometryCalculator::SwerveOdometryCalculator() {}
+
+SwerveOdometryOutput SwerveOdometryCalculator::calculate(
+    SwerveOdometryInputs inputs) {
+  auto module_diffs = inputs.drive_pos - previous_module_positions_;
+  previous_module_positions_ = inputs.drive_pos;
+
+  frc846::math::Vector2D displacement{0_ft, 0_ft};
+
+  for (int i = 0; i < 4; i++) {
+    displacement +=
+        frc846::math::Vector2D{inputs.drive_pos[i], inputs.steer_pos[i], true};
+  }
+  displacement.rotate(inputs.bearing, true);
+
+  last_position_ += displacement;
+
+  return {last_position_};
+}
+
+}  // namespace frc846::robot::swerve::odometry

src/frc846/cpp/frc846/robot/swerve/swerve_module.cc

@@ -20,13 +20,6 @@ SwerveModuleSubsystem::SwerveModuleSubsystem(Loggable& parent, std::string loc,
   drive_helper_.SetConversion(drive_reduction);
   steer_helper_.SetConversion(steer_reduction);
 
-  drive_.EnableStatusFrames(
-      {frc846::control::config::StatusFrame::kPositionFrame,
-          frc846::control::config::StatusFrame::kVelocityFrame});
-
-  steer_.EnableStatusFrames(
-      {frc846::control::config::StatusFrame::kPositionFrame});
-
   drive_helper_.bind(&drive_);
   steer_helper_.bind(&steer_);
 
@@ -37,7 +30,16 @@ SwerveModuleSubsystem::SwerveModuleSubsystem(Loggable& parent, std::string loc,
 }
 
 void SwerveModuleSubsystem::Setup() {
+  drive_.EnableStatusFrames(
+      {frc846::control::config::StatusFrame::kPositionFrame,
+          frc846::control::config::StatusFrame::kVelocityFrame});
+
   drive_.Setup();
+  drive_helper_.SetPosition(0_ft);
+
+  steer_.EnableStatusFrames(
+      {frc846::control::config::StatusFrame::kPositionFrame});
+
   steer_.Setup();
 
   ZeroWithCANcoder();

src/frc846/include/frc846/robot/GenericSubsystem.h

@@ -7,13 +7,11 @@
 
 namespace frc846::robot {
 
-#define FRC846_VERIFY(expr, ok, fail_msg)         \
-  do {                                            \
-    if (!(expr)) {                                \
-      ok = false;                                 \
-      Error("Verification failed: {}", fail_msg); \
-    }                                             \
-  } while (0)
+#define FRC846_VERIFY(expr, ok, fail_msg)       \
+  if (!(expr)) {                                \
+    ok = false;                                 \
+    Error("Verification failed: {}", fail_msg); \
+  }
 
 // Non-templated subsystem base class.
 class SubsystemBase : public frc846::base::Loggable {

src/frc846/include/frc846/robot/swerve/drivetrain.h

@@ -1,20 +1,29 @@
 #pragma once
 
+#include <AHRS.h>
 #include <units/acceleration.h>
 #include <units/angular_acceleration.h>
 
 #include "frc846/robot/GenericSubsystem.h"
+#include "frc846/robot/swerve/odometry/swerve_odometry_calculator.h"
 #include "frc846/robot/swerve/odometry/swerve_pose.h"
 #include "frc846/robot/swerve/swerve_module.h"
 
 namespace frc846::robot::swerve {
 
+enum NavX_connection_type {
+  kSPI,
+  kSerial,
+};
+
 /*
 DrivetrainConfigs
 
 Contains all configs related to the specific drivetrain in use.
 */
-struct DrivetrainConfigs {};
+struct DrivetrainConfigs {
+  NavX_connection_type navX_connection_mode;
+};
 
 struct DrivetrainReadings {
   frc846::robot::swerve::odometry::SwervePose pose;
@@ -43,7 +52,7 @@ class DrivetrainSubsystem
     : public frc846::robot::GenericSubsystem<DrivetrainReadings,
           DrivetrainTarget> {
 public:
-  DrivetrainSubsystem(DrivetrainConfigs configs, bool enable);
+  DrivetrainSubsystem(DrivetrainConfigs configs);
 
   void Setup() override;
 
@@ -58,6 +67,10 @@ class DrivetrainSubsystem
 
   DrivetrainConfigs configs_;
   std::array<SwerveModuleSubsystem*, 4> modules_;
+
+  AHRS* navX_;
+
+  frc846::robot::swerve::odometry::SwerveOdometryCalculator odometry_;
 };
 
 }  // namespace frc846::robot::swerve

src/frc846/include/frc846/robot/swerve/odometry/swerve_odometry_calculator.h

@@ -0,0 +1,39 @@
+#pragma once
+
+#include <units/angle.h>
+#include <units/length.h>
+
+#include <array>
+
+#include "frc846/math/calculator.h"
+#include "frc846/math/vectors.h"
+
+namespace frc846::robot::swerve::odometry {
+
+struct SwerveOdometryConstants {};
+
+struct SwerveOdometryInputs {
+  units::degree_t bearing;
+  std::array<units::degree_t, 4> steer_pos;
+  frc846::math::VectorND<units::inch, 4> drive_pos;
+};
+
+struct SwerveOdometryOutput {
+  frc846::math::Vector2D position;
+};
+
+class SwerveOdometryCalculator
+    : public frc846::math::Calculator<SwerveOdometryInputs,
+          SwerveOdometryOutput, SwerveOdometryConstants> {
+public:
+  SwerveOdometryCalculator();
+
+  SwerveOdometryOutput calculate(SwerveOdometryInputs inputs) override;
+
+private:
+  frc846::math::VectorND<units::inch, 4> previous_module_positions_;
+
+  frc846::math::Vector2D last_position_;
+};
+
+}  // namespace frc846::robot::swerve::odometry

style.standard

@@ -1 +1 @@
-{   BasedOnStyle: Google,   AccessModifierOffset: -2,   AlignAfterOpenBracket: DontAlign,   AlignConsecutiveAssignments: false,   AlignConsecutiveDeclarations: false,   AlignEscapedNewlines: Left,   AlignOperands: true,   AlignTrailingComments: true,   AllowAllParametersOfDeclarationOnNextLine: true,   AllowShortBlocksOnASingleLine: true,   AllowShortCaseLabelsOnASingleLine: true,   AllowShortEnumsOnASingleLine: true,   AllowShortFunctionsOnASingleLine: true,   AllowShortIfStatementsOnASingleLine: true,   AllowShortLambdasOnASingleLine: Inline,   AllowShortLoopsOnASingleLine: false,   AlwaysBreakAfterReturnType: None,   AlwaysBreakBeforeMultilineStrings: false,   AlwaysBreakTemplateDeclarations: false,   BinPackArguments: true,   BinPackParameters: true,   BreakAfterJavaFieldAnnotations: false,   BreakBeforeBinaryOperators: None,   BreakBeforeBraces: Attach,   BreakBeforeConceptDeclarations: false,   BreakBeforeTernaryOperators: true,   BreakConstructorInitializersBeforeComma: false,   ColumnLimit: 80,   CompactNamespaces: false,   ConstructorInitializerAllOnOneLineOrOnePerLine: true,   ConstructorInitializerIndentWidth: 4,   ContinuationIndentWidth: 4,   Cpp11BracedListStyle: true,   DeriveLineEnding: true,   DerivePointerAlignment: true,   DisableFormat: false,   FixNamespaceComments: false,   IndentAccessModifiers: false,   IndentCaseLabels: false,   IndentExternBlock: AfterExternBlock,   IndentPPDirectives: None,   IndentWidth: 2,   InsertTrailingCommas: None,   KeepEmptyLinesAtTheStartOfBlocks: false,   MacroBlockBegin: "",   MacroBlockEnd: "",   MaxEmptyLinesToKeep: 1,   NamespaceIndentation: None,   PenaltyBreakAssignment: 2,   PenaltyBreakBeforeFirstCallParameter: 19,   PenaltyBreakComment: 300,   PenaltyBreakFirstLessLess: 120,   PenaltyBreakString: 1000,   PenaltyExcessCharacter: 1000000,   PenaltyReturnTypeOnItsOwnLine: 60,   PointerAlignment: Left,   ReflowComments: true,   SortIncludes: true,   SortUsingDeclarations: true,   SpaceAfterCStyleCast: false,   SpaceAfterLogicalNot: false,   SpaceAfterTemplateKeyword: true,   SpaceAroundPointerQualifiers: Default,   SpaceBeforeAssignmentOperators: true,   SpaceBeforeCaseColon: false,   SpaceBeforeCpp11BracedList: false,   SpaceBeforeCtorInitializerColon: true,   SpaceBeforeInheritanceColon: true,   SpaceBeforeParens: ControlStatements,   SpaceBeforeRangeBasedForLoopColon: true,   SpaceBeforeSquareBrackets: false,   SpaceInEmptyBlock: false,   SpaceInEmptyParentheses: false,   SpacesBeforeTrailingComments: 2,   SpacesInAngles: false,   SpacesInCStyleCastParentheses: false,   SpacesInContainerLiterals: false,   SpacesInParentheses: false,   SpacesInSquareBrackets: false,   Standard: Auto,   TabWidth: 2,   UseCRLF: false,   UseTab: Never }
+{   BasedOnStyle: Google,   AccessModifierOffset: -2,   AlignAfterOpenBracket: DontAlign,   AlignConsecutiveAssignments: false,   AlignConsecutiveDeclarations: false,   AlignEscapedNewlines: Left,   AlignOperands: true,   AlignTrailingComments: true,   AllowAllParametersOfDeclarationOnNextLine: true,   AllowShortBlocksOnASingleLine: true,   AllowShortCaseLabelsOnASingleLine: true,   AllowShortEnumsOnASingleLine: true,   AllowShortFunctionsOnASingleLine: true,   AllowShortIfStatementsOnASingleLine: true,   AllowShortLambdasOnASingleLine: Inline,   AllowShortLoopsOnASingleLine: false,   AlwaysBreakAfterReturnType: None,   AlwaysBreakBeforeMultilineStrings: false,   AlwaysBreakTemplateDeclarations: false,   BinPackArguments: true,   BinPackParameters: true,   BreakAfterJavaFieldAnnotations: false,   BreakBeforeBinaryOperators: None,   BreakBeforeBraces: Attach,   BreakBeforeConceptDeclarations: false,   BreakBeforeTernaryOperators: true,   BreakConstructorInitializersBeforeComma: false,   ColumnLimit: 80,   CompactNamespaces: false,   ConstructorInitializerAllOnOneLineOrOnePerLine: true,   ConstructorInitializerIndentWidth: 4,   ContinuationIndentWidth: 4,   Cpp11BracedListStyle: true,   DeriveLineEnding: true,   DerivePointerAlignment: true,   DisableFormat: false,   FixNamespaceComments: false,   IndentAccessModifiers: false,   IndentCaseLabels: false,   IndentExternBlock: AfterExternBlock,   IndentPPDirectives: None,   IndentWidth: 2,   InsertTrailingCommas: None,   KeepEmptyLinesAtTheStartOfBlocks: false,   MacroBlockBegin: "",   MacroBlockEnd: "",   MaxEmptyLinesToKeep: 1,   NamespaceIndentation: None,   PenaltyBreakAssignment: 2,   PenaltyBreakBeforeFirstCallParameter: 19,   PenaltyBreakComment: 300,   PenaltyBreakFirstLessLess: 120,   PenaltyBreakString: 1000,   PenaltyExcessCharacter: 1000000,   PenaltyReturnTypeOnItsOwnLine: 60,   PointerAlignment: Left,   ReflowComments: true,   SortIncludes: true,   SortUsingDeclarations: true,   SpaceAfterCStyleCast: false,   SpaceAfterLogicalNot: false,   SpaceAfterTemplateKeyword: true,   SpaceAroundPointerQualifiers: Default,   SpaceBeforeAssignmentOperators: true,   SpaceBeforeCaseColon: false,   SpaceBeforeCpp11BracedList: false,   SpaceBeforeCtorInitializerColon: true,   SpaceBeforeInheritanceColon: true,   SpaceBeforeParens: ControlStatements,   SpaceBeforeRangeBasedForLoopColon: true,   SpaceBeforeSquareBrackets: false,   SpaceInEmptyBlock: false,   SpaceInEmptyParentheses: false,   SpacesBeforeTrailingComments: 2,   SpacesInAngles: false,   SpacesInCStyleCastParentheses: false,   SpacesInContainerLiterals: false,   SpacesInParentheses: false,   SpacesInSquareBrackets: false,   Standard: Auto,   TabWidth: 2,   UseCRLF: false,   UseTab: Never   }