From 7b58370a6d4174f2e007264c58bebe7868e7fdc3 Mon Sep 17 00:00:00 2001
From: kartofen <mladenovnasko0@gmail.com>
Date: Sun, 11 Feb 2024 22:48:38 +0200
Subject: imu added

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 33 ++++++++++++----------
 1 file changed, 18 insertions(+), 15 deletions(-)

(limited to 'src/main/java/frc/robot/subsystems/Drivetrain.java')

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 14c9d50..8f1b975 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -1,30 +1,31 @@
 package frc.robot.subsystems;
 
+import com.revrobotics.CANSparkMax;
+
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
 import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
 import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
-import edu.wpi.first.wpilibj.AnalogGyro;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
+import frc.robot.util.IMUGyro;
 
 public class Drivetrain extends SubsystemBase 
 {
-    private AnalogGyro m_gyro = Constants.gyro;
+    private IMUGyro m_gyro = Constants.gyro;
 
-    private PWMSparkMax m_motorL_leader = Constants.motorL_leader;
-    private PWMSparkMax m_motorL_follower = Constants.motorL_follower;
-    private PWMSparkMax m_motorR_leader = Constants.motorR_leader;
-    private PWMSparkMax m_motorR_follower = Constants.motorR_follower;
+    private CANSparkMax m_motorL_leader = Constants.motorL_leader;
+    private CANSparkMax m_motorL_follower = Constants.motorL_follower;
+    private CANSparkMax m_motorR_leader = Constants.motorR_leader;
+    private CANSparkMax m_motorR_follower = Constants.motorR_follower;
 
     private Encoder m_encoderL = Constants.encoderL;
     private Encoder m_encoderR = Constants.encoderR;
 
-    private SimpleMotorFeedforward m_feedforward = Constants.feedforward;
+    private SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 1);
 
     private final PIDController m_pidL = new PIDController(1, 0, 0);
     private final PIDController m_pidR = new PIDController(1, 0, 0);
@@ -36,10 +37,10 @@ public class Drivetrain extends SubsystemBase
 
     public Drivetrain()
     {
-        m_gyro.reset();        
+        m_gyro.reset();
 
-        m_motorL_leader.addFollower(m_motorL_follower);
-        m_motorR_leader.addFollower(m_motorR_follower);
+        m_motorR_follower.follow(m_motorR_leader);
+        m_motorL_follower.follow(m_motorL_leader);
 
         // perhaps invert
         // m_motorR_leader.setInverted(true);
@@ -51,7 +52,7 @@ public class Drivetrain extends SubsystemBase
         m_encoderR.reset();
 
         m_odometry = new DifferentialDriveOdometry(
-            m_gyro.getRotation2d(),
+            m_gyro.toRotation2d(),
             Constants.encoderL.getDistance(),
             Constants.encoderR.getDistance());
     }
@@ -79,10 +80,12 @@ public class Drivetrain extends SubsystemBase
     @Override
     public void periodic() 
     {
+        m_gyro.update();
+
         m_odometry.update(
-            Constants.gyro.getRotation2d(),
-            Constants.encoderL.getDistance(),
-            Constants.encoderR.getDistance());
+            m_gyro.toRotation2d(),
+            m_encoderL.getDistance(),
+            m_encoderR.getDistance());
     }
 
     @Override
-- 
cgit v1.2.3