Zero outputs when emergency is activated

FRC5190 · Feb 19, 2019 · c7c7777 · c7c7777
1 parent 86926ed
commit c7c7777
Show file tree

Hide file tree

Showing 6 changed files with 17 additions and 6 deletions.
diff --git a/src/main/kotlin/org/ghrobotics/frc2019/Controls.kt b/src/main/kotlin/org/ghrobotics/frc2019/Controls.kt
@@ -59,11 +59,11 @@ object Controls {
         state({ !isClimbing }) {
             // Elevator
             axisButton(1, 0.15) {
-                change(OpenLoopElevatorCommand(source.map { it.pow(2).withSign(-it) * 1.0 }))
+                change(OpenLoopElevatorCommand(source.map { it.pow(2).withSign(-it) * 0.5 }))
             }
             // Arm
             axisButton(5, 0.15) {
-                change(OpenLoopArmCommand(source.map { it.pow(2).withSign(-it) * 1.0 }))
+                change(OpenLoopArmCommand(source.map { it.pow(2).withSign(-it) * 0.5 }))
             }
 
             val backModifier = triggerAxisButton(GenericHID.Hand.kLeft)

diff --git a/src/main/kotlin/org/ghrobotics/frc2019/auto/routines/AutoRoutine.kt b/src/main/kotlin/org/ghrobotics/frc2019/auto/routines/AutoRoutine.kt
@@ -1,5 +1,6 @@
 package org.ghrobotics.frc2019.auto.routines
 
+import org.ghrobotics.frc2019.Robot
 import org.ghrobotics.frc2019.auto.Autonomous
 import org.ghrobotics.frc2019.subsystems.drive.DriveSubsystem
 import org.ghrobotics.lib.commands.DelayCommand
@@ -20,7 +21,7 @@ abstract class AutoRoutine : Source<FalconCommand> {
             DriveSubsystem.localization.reset(Autonomous.startingPosition().pose)
         }
         +routine
-    }
+    }.withExit { Robot.emergencyActive }
 
     protected fun executeFor(time: Time, command: FalconCommand) = sequential {
         +command

diff --git a/src/main/kotlin/org/ghrobotics/frc2019/subsystems/arm/ArmSubsystem.kt b/src/main/kotlin/org/ghrobotics/frc2019/subsystems/arm/ArmSubsystem.kt
@@ -166,7 +166,10 @@ object ArmSubsystem : FalconSubsystem(), EmergencyHandleable {
     }
 
     // Emergency Management
-    override fun activateEmergency() = zeroClosedLoopGains()
+    override fun activateEmergency() {
+        zeroOutputs()
+        zeroClosedLoopGains()
+    }
 
     override fun recoverFromEmergency() = setClosedLoopGains()
 }
diff --git a/src/main/kotlin/org/ghrobotics/frc2019/subsystems/climb/ClimbSubsystem.kt b/src/main/kotlin/org/ghrobotics/frc2019/subsystems/climb/ClimbSubsystem.kt
@@ -164,6 +164,9 @@ object ClimbSubsystem : FalconSubsystem(), EmergencyHandleable {
     }
 
 
-    override fun activateEmergency() = zeroClosedLoopGains()
+    override fun activateEmergency() {
+        zeroOutputs()
+        zeroClosedLoopGains()
+    }
     override fun recoverFromEmergency() = setClosedLoopGains()
 }
diff --git a/src/main/kotlin/org/ghrobotics/frc2019/subsystems/drive/DriveSubsystem.kt b/src/main/kotlin/org/ghrobotics/frc2019/subsystems/drive/DriveSubsystem.kt
@@ -145,6 +145,7 @@ object DriveSubsystem : TankDriveSubsystem(), EmergencyHandleable {
     }*/
 
     override fun activateEmergency() {
+        zeroOutputs()
         leftGearbox.zeroClosedLoopGains()
         rightGearbox.zeroClosedLoopGains()
     }

diff --git a/src/main/kotlin/org/ghrobotics/frc2019/subsystems/elevator/ElevatorSubsystem.kt b/src/main/kotlin/org/ghrobotics/frc2019/subsystems/elevator/ElevatorSubsystem.kt
@@ -207,7 +207,10 @@ object ElevatorSubsystem : FalconSubsystem(), EmergencyHandleable {
     }
 
     // Emergency Management
-    override fun activateEmergency() = zeroClosedLoopGains()
+    override fun activateEmergency() {
+        zeroOutputs()
+        zeroClosedLoopGains()
+    }
 
     override fun recoverFromEmergency() = setClosedLoopGains()
 }