From 77cd156513deb1bfe90d72ce0a038b4b9d623aa7 Mon Sep 17 00:00:00 2001
From: Alex Saveau <saveau.alexandre@gmail.com>
Date: Sat, 3 Mar 2018 19:52:25 -0800
Subject: [PATCH] Kill emergency abort

Signed-off-by: Alex Saveau <saveau.alexandre@gmail.com>
---
 .../sert2521/powerup/drivetrain/Drivetrain.kt |  6 +--
 .../drivetrain/commands/EmergencyAbort.kt     | 38 -------------------
 .../powerup/drivetrain/commands/FixedDrive.kt | 19 ----------
 3 files changed, 1 insertion(+), 62 deletions(-)
 delete mode 100644 src/main/java/org/sert2521/powerup/drivetrain/commands/EmergencyAbort.kt
 delete mode 100644 src/main/java/org/sert2521/powerup/drivetrain/commands/FixedDrive.kt

diff --git a/src/main/java/org/sert2521/powerup/drivetrain/Drivetrain.kt b/src/main/java/org/sert2521/powerup/drivetrain/Drivetrain.kt
index c1a1928..f199d17 100644
--- a/src/main/java/org/sert2521/powerup/drivetrain/Drivetrain.kt
+++ b/src/main/java/org/sert2521/powerup/drivetrain/Drivetrain.kt
@@ -5,7 +5,6 @@ import com.kauailabs.navx.frc.AHRS
 import edu.wpi.first.wpilibj.I2C
 import edu.wpi.first.wpilibj.drive.DifferentialDrive
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
-import org.sert2521.powerup.drivetrain.commands.EmergencyAbort
 import org.sert2521.powerup.drivetrain.commands.TeleopDrive
 import org.sert2521.powerup.util.LEFT_FRONT_MOTOR
 import org.sert2521.powerup.util.LEFT_REAR_MOTOR
@@ -44,10 +43,7 @@ object Drivetrain : Subsystem() {
         rightDrive.setSelectedSensor(FeedbackDevice.QuadEncoder)
     }
 
-    override fun onStart() {
-        EmergencyAbort().start()
-        reset()
-    }
+    override fun onStart() = reset()
 
     override fun execute() {
         SmartDashboard.putNumber("Drivetrain Left Position", leftPosition.toDouble())
diff --git a/src/main/java/org/sert2521/powerup/drivetrain/commands/EmergencyAbort.kt b/src/main/java/org/sert2521/powerup/drivetrain/commands/EmergencyAbort.kt
deleted file mode 100644
index e04f8ab..0000000
--- a/src/main/java/org/sert2521/powerup/drivetrain/commands/EmergencyAbort.kt
+++ /dev/null
@@ -1,38 +0,0 @@
-package org.sert2521.powerup.drivetrain.commands
-
-import org.sert2521.powerup.drivetrain.Drivetrain
-import org.sert2521.powerup.drivetrain.Drivetrain.leftSpeed
-import org.sert2521.powerup.drivetrain.Drivetrain.rightSpeed
-import org.sertain.command.Command
-import kotlin.math.absoluteValue
-import kotlin.math.sign
-
-/**
- * Interrupts the current drivetrain command if the navX angle is above a specified amount in order
- * to prevent the robot from tipping over and dying a tragic death.
- */
-class EmergencyAbort : Command() {
-    private var history = listOf(0F, 0F, 0F, 0F, 0F, 0F, 0F, 0F)
-
-    override fun execute(): Boolean {
-        history += Drivetrain.ahrs.roll
-        history = history.takeLast(8)
-
-        val avg = history.average()
-        if (avg.absoluteValue >= TIP_THRESHOLD) {
-            println("EMERGENCY ABORT! ${avg.absoluteValue}")
-            // Reduce speed until we aren't dying
-            FixedDrive(
-                    leftSpeed - REDUCTION_SPEED * avg.sign,
-                    rightSpeed - REDUCTION_SPEED * avg.sign
-            ).start()
-        }
-
-        return false
-    }
-
-    private companion object {
-        const val TIP_THRESHOLD = 17
-        const val REDUCTION_SPEED = 0.01
-    }
-}
diff --git a/src/main/java/org/sert2521/powerup/drivetrain/commands/FixedDrive.kt b/src/main/java/org/sert2521/powerup/drivetrain/commands/FixedDrive.kt
deleted file mode 100644
index 51eed41..0000000
--- a/src/main/java/org/sert2521/powerup/drivetrain/commands/FixedDrive.kt
+++ /dev/null
@@ -1,19 +0,0 @@
-package org.sert2521.powerup.drivetrain.commands
-
-import org.sert2521.powerup.drivetrain.Drivetrain
-import org.sertain.command.Command
-
-/**
- * Drives at a specified fixed speed for a single tick while cancelling all other [Drivetrain]
- * commands.
- */
-class FixedDrive(private val leftSpeed: Double, private val rightSpeed: Double) : Command() {
-    init {
-        requires(Drivetrain)
-    }
-
-    override fun execute(): Boolean {
-        Drivetrain.drive(leftSpeed, rightSpeed)
-        return true
-    }
-}