From 09310a745f167d521dec4acccea4447df49901ec Mon Sep 17 00:00:00 2001
From: Zynh0722 <Zynh0722@gmail.com>
Date: Wed, 27 Mar 2024 17:31:15 -0700
Subject: [PATCH] encoder and pid loops

---
 src/main/cpp/cowlib/SwerveModule.cpp   |  7 ++++++-
 src/main/include/Constants.h           | 14 +++++++++++++-
 src/main/include/RobotContainer.h      |  2 +-
 src/main/include/cowlib/SwerveModule.h |  2 ++
 4 files changed, 22 insertions(+), 3 deletions(-)

diff --git a/src/main/cpp/cowlib/SwerveModule.cpp b/src/main/cpp/cowlib/SwerveModule.cpp
index c77f096..5cad1da 100644
--- a/src/main/cpp/cowlib/SwerveModule.cpp
+++ b/src/main/cpp/cowlib/SwerveModule.cpp
@@ -2,11 +2,16 @@
 // 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 "Constants.h"
 #include "rev/CANSparkMax.h"
 
 #include "cowlib/SwerveModule.h"
 
+using namespace SwerveConstants;
+
+using MotorType = rev::CANSparkMax::MotorType;
+
 SwerveModule::SwerveModule(int driveMotorId, int steerMotorId, int encoderId)
     : driveMotor{driveMotorId, rev::CANSparkMax::MotorType::kBrushless},
       steerMotor{steerMotorId, rev::CANSparkMax::MotorType::kBrushless},
-      encoder{encoderId} {}
+      encoder{encoderId}, pid{SwervePID::p, SwervePID::i, SwervePID::d} {}
diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h
index 7d374ec..6803cd2 100644
--- a/src/main/include/Constants.h
+++ b/src/main/include/Constants.h
@@ -16,6 +16,18 @@
 
 namespace OperatorConstants {
 
-inline constexpr int kDriverControllerPort = 0;
+inline constexpr int driveControllerPort = 0;
 
 } // namespace OperatorConstants
+
+namespace SwerveConstants {
+
+namespace SwervePID {
+
+inline constexpr double p = 0.12;
+inline constexpr double i = 0;
+inline constexpr double d = 0.0015;
+
+}; // namespace SwervePID
+
+} // namespace SwerveConstants
diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h
index ce4236d..1056414 100644
--- a/src/main/include/RobotContainer.h
+++ b/src/main/include/RobotContainer.h
@@ -26,7 +26,7 @@ public:
 private:
   // Replace with CommandPS4Controller or CommandJoystick if needed
   frc2::CommandXboxController m_driverController{
-      OperatorConstants::kDriverControllerPort};
+      OperatorConstants::driveControllerPort};
 
   // The robot's subsystems are defined here...
   ExampleSubsystem m_subsystem;
diff --git a/src/main/include/cowlib/SwerveModule.h b/src/main/include/cowlib/SwerveModule.h
index 95365f4..fd348a3 100644
--- a/src/main/include/cowlib/SwerveModule.h
+++ b/src/main/include/cowlib/SwerveModule.h
@@ -5,6 +5,7 @@
 #pragma once
 
 #include <ctre/phoenix6/CANcoder.hpp>
+#include <frc/controller/PIDController.h>
 #include <rev/CANSparkMax.h>
 
 class SwerveModule {
@@ -12,6 +13,7 @@ private:
   rev::CANSparkMax driveMotor;
   rev::CANSparkMax steerMotor;
   ctre::phoenix6::hardware::CANcoder encoder;
+  frc::PIDController pid;
 
 public:
   SwerveModule(int driveMotorId, int steerMotorId, int encoderId);