From 2756df8e1bbc4828b223a41b23718255662756cc Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Wed, 26 Feb 2020 08:41:10 -0700
Subject: [PATCH] Simplify Periodic to make it easier to read

---
 src/main/java/frc4388/robot/subsystems/Drive.java | 15 ++++++++++++---
 1 file changed, 12 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index fca1e7b..d9f1fb2 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -273,14 +273,26 @@ public class Drive extends SubsystemBase {
 
   @Override
   public void periodic() {
+    updateTime();
+    updateAngles();
+    updatePosition();
+    runFalconCooling();
+    updateSmartDashboard();
+  }
+
+  public void updateTime() {
     m_lastTimeMs = m_currentTimeMs;
     m_currentTimeMs = System.currentTimeMillis();
     m_currentTimeSec = m_currentTimeMs / 1000;
     m_deltaTimeMs = m_currentTimeMs - m_lastTimeMs;
+  }
 
+  public void updateAngles() {
     m_lastAngleYaw = m_currentAngleYaw;
     m_currentAngleYaw = getGyroYaw();
+  }
 
+  public void updatePosition() {
     m_rightFrontMotorPos = m_rightFrontMotor.getSelectedSensorPosition();
     m_rightFrontMotorVel = m_rightFrontMotor.getSelectedSensorVelocity();
 
@@ -295,9 +307,6 @@ public class Drive extends SubsystemBase {
     m_odometry.update(Rotation2d.fromDegrees( getHeading()),
                                               getDistanceInches(m_leftFrontMotor),
                                               -getDistanceInches(m_rightFrontMotor));
-
-    runFalconCooling();
-    updateSmartDashboard();
   }
 
   /**