Made slight changes to make intake more consistent

Author: EvanWoodfin

src/main/java/frc/robot/RobotContainer.java

@@ -21,6 +21,7 @@
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
@@ -240,8 +241,8 @@ private Command robotGoToHeightCommandCreator(ElevatorHeight height){
         }else{
             command = Commands.sequence(
                 new WristGoToPositionCommand(wrist, WristAngleRad.L2L3),
-                new ElevatorGoToPositionPositionCommand(elevator, height)
-                //new WristGoToPositionCommand(wrist, getEndWristAngleForGivenElevatorHeight(height))
+                new ElevatorGoToPositionPositionCommand(elevator, height),
+                new WristGoToPositionCommand(wrist, getEndWristAngleForGivenElevatorHeight(height))
             ).withInterruptBehavior(InterruptionBehavior.kCancelSelf);
         }
 
@@ -253,7 +254,7 @@ private Command robotGoToHeightCommandCreator(ElevatorHeight height){
     private WristAngleRad getEndWristAngleForGivenElevatorHeight(ElevatorHeight height){
         switch(height){
             case HOME:
-                return WristAngleRad.FREEHANG;
+                return WristAngleRad.L2L3;
             case FUNNEL:
                 return WristAngleRad.FUNNEL_ANGLE;
             case L1:
@@ -378,6 +379,14 @@ private void configureManipulatorBindings(){
 
         Trigger outtakeCoralTrigger = operatorController.rightBumper();
         outtakeCoralTrigger.whileTrue(new OuttakeCoralCommand(manipulator));
+
+        Trigger takeCoralBackTrigger = operatorController.leftTrigger();
+        takeCoralBackTrigger.whileTrue(
+            Commands.sequence(
+            new ManipulatorIntakeCoralCommand(manipulator),
+            new WaitCommand(0.04),
+            Commands.runOnce(() -> manipulator.stopOuttake(), manipulator)));
+
     }
 
     private void configureElevatorBindings(){

src/main/java/frc/robot/commands/ManipulatorIntakeCoralCommand.java

@@ -29,7 +29,9 @@ public void execute() {
 
   @Override
   public void end(boolean interrupted) {
-    manipulator.stopOuttake();
+    if (interrupted) {
+      manipulator.stopOuttake();
+    }
   }
 
   @Override

src/main/java/frc/robot/subsystems/Manipulator/Manipulator.java

@@ -74,6 +74,9 @@ public void runOuttakeOut(){
   public void runOuttakeIn(){
     manipulatorIO.runOuttakeVoltage(Constants.ManipulatorConstants.intakeVoltage);
   }
+  public void runOuttakeInReverse(){
+    manipulatorIO.runOuttakeVoltage(-Constants.ManipulatorConstants.intakeVoltage);
+  }
   public void stopOuttake(){
     manipulatorIO.stopOuttake();
   }

src/main/java/frc/robot/util/FunnelIntakeCommands.java

@@ -4,6 +4,7 @@
 import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.ConditionalCommand;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.Constants.ElevatorConstants.ElevatorHeight;
 import frc.robot.Constants.WristConstants.WristAngleRad;
 import frc.robot.commands.ManipulatorIntakeCoralCommand;
@@ -46,6 +47,8 @@ private static Command whichFunnelIntakeCommand(Elevator elevator, Wrist wrist,
             new ElevatorGoToPositionPositionCommand(elevator, ElevatorHeight.FUNNEL),
             new WristGoToPositionCommand(wrist, WristAngleRad.FUNNEL_ANGLE),
             new ManipulatorIntakeCoralCommand(manipulator),
+            new WaitCommand(0.08),
+            Commands.runOnce(() -> manipulator.stopOuttake(), manipulator),
             new WristGoToPositionCommand(wrist, WristAngleRad.L2L3),
             new ElevatorGoToPositionPositionCommand(elevator, ElevatorHeight.HOME)
         );