From 4e2ce92c6edfaca3f96bccafbb8d8a5039d10a10 Mon Sep 17 00:00:00 2001
From: Andrew Dassonville <dassonville.andrew@gmail.com>
Date: Sat, 11 Aug 2018 01:46:48 -0700
Subject: [PATCH] Use notifier class for path following

---
 .../sert2521/powerup/autonomous/Autonomous.kt | 20 ++++++++++++-------
 1 file changed, 13 insertions(+), 7 deletions(-)

diff --git a/src/main/java/org/sert2521/powerup/autonomous/Autonomous.kt b/src/main/java/org/sert2521/powerup/autonomous/Autonomous.kt
index 435b8a6..1abc8de 100644
--- a/src/main/java/org/sert2521/powerup/autonomous/Autonomous.kt
+++ b/src/main/java/org/sert2521/powerup/autonomous/Autonomous.kt
@@ -1,5 +1,6 @@
 package org.sert2521.powerup.autonomous
 
+import edu.wpi.first.wpilibj.Notifier
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
 import jaci.pathfinder.Pathfinder
 import jaci.pathfinder.followers.EncoderFollower
@@ -136,6 +137,7 @@ object Auto : RobotLifecycle {
 
 private abstract class PathFollowerBase(protected val path: PathBase) : Command() {
     private val pathIndex get() = pathIndexField.get(path.left) as Int
+    private val notifier = Notifier { followPath() }
 
     init {
         requires(Drivetrain)
@@ -153,9 +155,18 @@ private abstract class PathFollowerBase(protected val path: PathBase) : Command(
             right.configureEncoder(0, ENCODER_TICKS_PER_REVOLUTION, WHEEL_DIAMETER)
             right.configurePIDVA(1.0, 0.0, 0.05, 1 / MAX_VELOCITY, 0.0)
         }
+
+        notifier.startPeriodic(PERIOD)
+    }
+
+    override fun execute() = path.isFinished
+
+    override fun onDestroy() {
+        pathProgress = null
+        notifier.stop()
     }
 
-    override fun execute(): Boolean {
+    private fun followPath() {
         val leftPosition = Drivetrain.leftPosition
         val rightPosition = Drivetrain.rightPosition
 
@@ -166,12 +177,6 @@ private abstract class PathFollowerBase(protected val path: PathBase) : Command(
         calculate(leftPosition, rightPosition, turn).apply { drive(first, second) }
 
         pathProgress = pathIndex.toDouble() / path.trajectory.segments.size.toDouble()
-
-        return path.isFinished
-    }
-
-    override fun onDestroy() {
-        pathProgress = null
     }
 
     protected open fun calculate(leftPosition: Int, rightPosition: Int, turn: Double) =
@@ -183,6 +188,7 @@ private abstract class PathFollowerBase(protected val path: PathBase) : Command(
 
     private companion object {
         const val TURN_IMPORTANCE = 0.0005
+        const val PERIOD = 0.01 // 100Hz
 
         val pathIndexField: Field = EncoderFollower::class.java.getDeclaredField("segment").apply {
             isAccessible = true