Testing on 3/31

Author: ExamWeHaltLitMaTowel

src/main/java/frc/robot/HeadHoncho.java

@@ -374,6 +374,21 @@ public void initialize() {
         LIFT_SUBSYSTEM.setState(TargetLiftStates.STOW);
       }
 
+      @Override
+      public SystemState nextState() {
+        if (s_cancelButton.getAsBoolean()) return STOW;
+        if (timer.hasElapsed(0.5)) return ALGAE_KICK;
+        return this;
+      }
+    },
+    ALGAE_KICK {
+      Timer timer = new Timer();
+      @Override
+      public void initialize() {
+        timer.restart();
+        LIFT_SUBSYSTEM.setState(TargetLiftStates.A_KICK);
+      }
+
       @Override
       public SystemState nextState() {
         if (s_cancelButton.getAsBoolean()) return STOW;
@@ -389,6 +404,7 @@ public void initialize() {
 
       @Override
       public SystemState nextState() {
+        if (s_cancelButton.getAsBoolean()) return STOW;
         if (END_EFFECTOR_SUBSYSTEM.isEmpty()) return INTAKE;
 
         return this;
@@ -410,6 +426,7 @@ public void initialize() {
 
       @Override
       public SystemState nextState() {
+        if (s_cancelButton.getAsBoolean()) return STOW;
         if (END_EFFECTOR_SUBSYSTEM.isEmpty()) return INTAKE;
 
         return this;

src/main/java/frc/robot/Robot.java

@@ -5,6 +5,7 @@
 package frc.robot;
 
 import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.wpilibj.Servo;
 import edu.wpi.first.wpilibj.Threads;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -20,8 +21,12 @@ public class Robot extends LoggedRobot {
 
   private final RobotContainer m_robotContainer;
 
+  private final Servo phlapServo;
+
   public Robot() {
 
+    phlapServo = new Servo(3);
+
     if (isReal()) {
       Logger.addDataReceiver(new WPILOGWriter("/U/")); // Log to a USB stick ("/U/logs")
       Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
@@ -67,6 +72,8 @@ public void disabledExit() {}
   public void autonomousInit() {
     Logger.recordOutput("Auto/Lift/State", "starting");
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+    
+    phlapServo.set(1);
 
     if (m_autonomousCommand != null) {
       m_autonomousCommand.schedule();
@@ -83,6 +90,8 @@ public void autonomousExit() {}
 
   @Override
   public void teleopInit() {
+    phlapServo.set(1);
+
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }

src/main/java/frc/robot/subsystems/drivetrain/DriveSubsystem.java

@@ -395,7 +395,7 @@ public void end(boolean interrupted) {
 
   // private static ArrayList<AprilTagCamera> m_cameras;
 
-  private static double s_driveSpeedScalar = 0.2;
+  private static double s_driveSpeedScalar = Constants.Drive.FAST_SPEED_SCALAR;
 
   protected final Thread m_limelight_thread;
 
@@ -429,8 +429,8 @@ public DriveSubsystem(Hardware driveHardware, Telemetry logger) {
     s_autoDrive.HeadingController.setPID(5, 0, 0);
     s_autoDrive.HeadingController.enableContinuousInput(0, Math.PI * 2);
 
-    s_autoDrive.XController.setPID(1.5, 0, 0);
-    s_autoDrive.YController.setPID(4, 0, 0);
+    s_autoDrive.XController.setPID(6, 0, 0);
+    s_autoDrive.YController.setPID(6, 0, 0);
 
     s_drivetrain.registerTelemetry(logger::telemeterize);
 

src/main/java/frc/robot/subsystems/lift/LiftSubsystem.java

@@ -56,7 +56,8 @@ public static enum TargetLiftStates {
     L4,
     A1,
     A2,
-    A_SCORE
+    A_SCORE,
+    A_KICK
   }
 
   private static TargetLiftStates nextState;
@@ -211,6 +212,9 @@ public SystemState nextState() {
         if (nextState == TargetLiftStates.TURBO) {
           return STOW_TURBO_S1;
         }
+        if (nextState == TargetLiftStates.A_KICK) {
+          return A_KICK;
+        }
         return this;
       }
     },
@@ -439,6 +443,34 @@ public SystemState nextState() {
         if (nextState == TargetLiftStates.STOW) {
           return STOW;
         }
+        if (nextState == TargetLiftStates.A_KICK) {
+          return A_KICK;
+        }
+        return this;
+      }
+    },
+    A_KICK {
+      @Override
+      public void initialize() {
+        s_liftinstance.setElevatorHeight(STOW_HEIGHT);
+        s_liftinstance.setArmAngle(SCORING_A1_ANGLE);
+      }
+
+      @Override
+      public void execute() {
+        if (s_liftinstance.armAt(SCORING_A1_ANGLE) && s_liftinstance.elevatorAt(STOW_HEIGHT)) {
+          isLiftReady = true;
+        } else {
+          isLiftReady = false;
+        }
+      }
+
+      @Override
+      public SystemState nextState() {
+        curState = TargetLiftStates.A_KICK;
+        if (nextState == TargetLiftStates.STOW) {
+          return STOW;
+        }
         return this;
       }
     },