From 35e8b14f9d6fd5d95f1ca329dc5e3eecb0cefc6f Mon Sep 17 00:00:00 2001
From: JustBeChill <kmalling16@gmail.com>
Date: Mon, 2 Oct 2023 17:03:48 -0700
Subject: [PATCH] Safety mode removed

---
 src/main/java/frc/robot/Robot.java            | 59 -------------------
 .../java/frc/robot/subsystems/Elevator.java   | 11 ----
 .../java/frc/robot/systems/ElevFourbar.java   |  5 --
 3 files changed, 75 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 9aae258..79169b4 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -145,7 +145,6 @@ public void teleopInit() {
         SwerveDrive.setMode(SwerveMode.HEADLESS);
         ElevFourbar.init();
         Intake.init();
-        ElevFourbar.enableSafetyMode(false);
     }
     
     /** This function is called periodically during operator control. */
@@ -210,64 +209,6 @@ public void disabledPeriodic() {
         ElevFourbar.toggleGamePiece(controlPanel.getRawButtonReleased(5)); 
     }
     
-    /** This  function is called once when test mode is enabled. */
-    @Override
-    public void testInit() {
-        controlMode = ControlMode.SAFETY;
-        SwerveDrive.setMode(SwerveMode.HEADLESS);
-        ElevFourbar.init();
-        Intake.init();
-        ElevFourbar.enableSafetyMode(true);
-    }
-    
-    /** This function is called periodically during test mode. */
-    @Override
-    public void testPeriodic() {
-        SmartDashboard.putNumber("controller Y", controllerOne.getLeftY());
-        SmartDashboard.putNumber("controller X", controllerOne.getLeftX());
-
-        // Toggle translation curve
-        if (controllerOne.getLeftBumperReleased()) {
-            translationLimiter.toggleEnabled();
-        }
-
-        double x = controllerOne.getLeftX();
-        double y = controllerOne.getLeftY();
-
-        //SCALES DOWN DRIVE SPEED FOR 
-        SwerveDrive.run(x, y, controllerOne.getRightX(), controllerOne.getPOV());
-        SwerveDrive.conditionalTempMode(SwerveMode.ROCK, controllerOne.getRightTriggerAxis() > 0.25);
-        
-        // Left stick changes between headless and relative control modes.
-        if (controllerOne.getLeftStickButtonReleased()) {
-            if (SwerveDrive.getMode() == SwerveMode.HEADLESS) {
-                SwerveDrive.setMode(SwerveMode.RELATIVE);
-            } else {
-                SwerveDrive.setMode(SwerveMode.HEADLESS);
-            }
-        }
-        
-        Intake.run(
-            controlPanel.getRawButtonPressed(8), //controller one dpad to control pivot
-            controlPanel.getRawButton(9),
-            controlPanel.getRawButton(7),
-            controlPanel.getRawButton(6),
-            controlPanel.getRawButtonReleased(6)
-        );
-        
-        ElevFourbar.run(
-            controllerTwo.getRightY(),
-            Math.abs(controlPanel.getRawAxis(0) / 2) > Math.abs(controllerTwo.getLeftY()) ? controlPanel.getRawAxis(0) / 2 : -controllerTwo.getLeftY(),
-            controllerTwo.getPOV(),
-            controlPanel.getRawButtonPressed(5), //toggle game piece
-            controlPanel.getRawButtonReleased(1), //ground
-            controlPanel.getRawButtonReleased(3), //mid
-            controlPanel.getRawButtonReleased(4), //high
-            controlPanel.getRawButtonReleased(2),  //stowed
-            controllerTwo.getStartButtonPressed() //zero elevator encoder
-        );
-    }
-    
     /** This function is called once when the robot is first started up. */
     @Override
     public void simulationInit() {}
diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java
index 3750e53..1423877 100644
--- a/src/main/java/frc/robot/subsystems/Elevator.java
+++ b/src/main/java/frc/robot/subsystems/Elevator.java
@@ -74,17 +74,6 @@ public void init() {
 
     }
 
-    public void enableSafetyMode(boolean enable) {
-        if (enable) {
-            elevatorMotor.configPeakOutputForward(0.4);
-            elevatorMotor.configPeakOutputReverse(-0.4);
-        } else {
-            elevatorMotor.configPeakOutputForward(0.85);
-            elevatorMotor.configPeakOutputReverse(-0.85);
-        }
-        
-    }
-
     public void autonomousInit() {
         targetSetpoint = 0;
     }
diff --git a/src/main/java/frc/robot/systems/ElevFourbar.java b/src/main/java/frc/robot/systems/ElevFourbar.java
index 28c6348..efa504c 100644
--- a/src/main/java/frc/robot/systems/ElevFourbar.java
+++ b/src/main/java/frc/robot/systems/ElevFourbar.java
@@ -85,11 +85,6 @@ public static void autonomousInit() {
         elevator.autonomousInit();
         fourbar.setPIDSpeed(0.3);
     }
-    /** Halves the speed of fourbar and elevator  */
-    public static void enableSafetyMode(boolean enable) {
-        elevator.enableSafetyMode(enable);
-        fourbar.enableSafetyMode(enable);
-    }
 
     public static void run(double elevatorSpeed, double fourbarSpeed, int dPadDirection, boolean toggleGamePieceMode, boolean groundIntake, boolean mid, boolean high, boolean zeroElevEncoder) {
         instance.currentSetpoint = new Setpoint(false, elevator.getPosition(), fourbar.getPosition());