Fix DriveToCube command

This allows the DriveToCube command to have no cube in sight for a set
amount of time without terminating. This allows for the robot to
occasionally lose sight without killing auto.

src/main/java/org/sert2521/powerup/drivetrain/commands/DriveToCube.kt

@@ -5,26 +5,35 @@ import org.sert2521.powerup.elevator.Elevator
 import org.sert2521.powerup.intake.Intake
 import org.sert2521.powerup.util.DEGREES_PER_PIXEL
 import org.sert2521.powerup.util.Vision
+import java.util.Date
+
+class DriveToCube : AngleDriver(0.008, 0.0, 0.022) {
+    private lateinit var visionLastSeen: Date
 
-class DriveToCube : AngleDriver(0.01, 0.0, 0.022) {
     init {
         requires(Drivetrain)
     }
 
-    override fun onCreate() = updateSetpoint(0.0)
+    override fun onCreate() {
+        updateSetpoint(0.0)
+        visionLastSeen = Date()
+    }
 
     override fun execute(output: Double): Boolean {
         println("Found Cube: ${Vision.found}, " +
                 "X Offset: ${Vision.xOffset}, " +
-                "Y Offset: ${Vision.yOffset}")
+                "Y Offset: ${Vision.yOffset}, " +
+                "Values: (${BASE_SPEED + output}, ${BASE_SPEED - output})")
 
         if (Vision.found == true) {
+            visionLastSeen = Date()
+
             Drivetrain.drive(BASE_SPEED + output, BASE_SPEED - output)
             updateSetpoint(Vision.xOffset?.toDouble() ?: 0.0 * DEGREES_PER_PIXEL)
             return Intake.hasCube && Elevator.atBottom
         }
 
-        return true
+        return visionLastSeen.time - Date().time > MAX_TIME_WITHOUT_CUBE
     }
 
     private fun updateSetpoint(offset: Double) {
@@ -33,5 +42,6 @@ class DriveToCube : AngleDriver(0.01, 0.0, 0.022) {
 
     private companion object {
         const val BASE_SPEED = 0.3
+        const val MAX_TIME_WITHOUT_CUBE = 5000
     }
 }