finish score L1

Author: rift10

src/main/java/frc/robot/commands/compound/IntakeCoral.java

@@ -9,9 +9,6 @@
 public class IntakeCoral extends SequentialCommandGroup {
 
     public IntakeCoral(RobotContainer robot) {
-        addCommands(
-                new RunIntake(robot),
-                new AlignForHandoff(robot),
-                new HandoffCoral(robot));
+        addCommands(new RunIntake(robot), new AlignForHandoff(robot), new HandoffCoral(robot));
     }
 }

src/main/java/frc/robot/commands/compound/ScoreL1.java

@@ -2,9 +2,9 @@
 
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.RobotContainer;
-import frc.robot.commands.intake.intakeroller.OuttakeCoral;
+import frc.robot.commands.intake.OuttakeL1;
+import frc.robot.commands.intake.intakepivot.IntakePivotToPosition;
 import frc.robot.subsystems.intake.pivot.IntakePivot;
 import frc.robot.subsystems.intake.pivot.IntakePivot.IntakePivotGoal;
 
@@ -17,8 +17,8 @@ public ScoreL1(RobotContainer robot) {
         addRequirements(intakePivot);
 
         addCommands(
-                new InstantCommand(() -> intakePivot.runPosition(IntakePivotGoal.TO_SCOREL1)),
-                new WaitCommand(0.7),
-                new OuttakeCoral(robot));
+                new IntakePivotToPosition(robot, () -> IntakePivotGoal.TO_SCOREL1),
+                new OuttakeL1(robot),
+                new InstantCommand(() -> robot.getIntakePivot().setCurrentGoal(IntakePivotGoal.TO_INTAKE_HANDOFF)));
     }
 }

src/main/java/frc/robot/commands/intake/AlignForHandoff.java

@@ -30,9 +30,10 @@ public void execute() {}
 
     @Override
     public boolean isFinished() {
-        return pivot.getCurrentGoal() == IntakePivotGoal.TO_INTAKE_HANDOFF && 
-                wrist.getCurrentGoal() == WristGoal.HANDOFF &&
-                pivot.atGoal() && wrist.atGoal();
+        return pivot.getCurrentGoal() == IntakePivotGoal.TO_INTAKE_HANDOFF
+                && wrist.getCurrentGoal() == WristGoal.HANDOFF
+                && pivot.atGoal()
+                && wrist.atGoal();
     }
 
     @Override

src/main/java/frc/robot/commands/intake/HandoffCoral.java

@@ -33,18 +33,17 @@ public void initialize() {
 
     @Override
     public void execute() {
-        roller.setGoal(IntakeRollerGoal.OUTTAKEENDEFFECTOR);
+        roller.setCurrentGoal(IntakeRollerGoal.OUTTAKEENDEFFECTOR);
         endEffector.setGoal(EndEffectorRollerGoal.INTAKE);
     }
 
     @Override
     public boolean isFinished() {
-        return (beamBreak.gamepieceInEndEffector() && !beamBreak.coralInIntake())
-                || timer.hasElapsed(kTimeOutTime);
+        return (beamBreak.gamepieceInEndEffector() && !beamBreak.coralInIntake()) || timer.hasElapsed(kTimeOutTime);
     }
 
     @Override
     public void end(boolean isFinished) {
-        roller.setGoal(IntakeRollerGoal.IDLE);
+        roller.setCurrentGoal(IntakeRollerGoal.IDLE);
     }
 }

src/main/java/frc/robot/commands/intake/OuttakeL1.java

@@ -0,0 +1,38 @@
+package frc.robot.commands.intake;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.beambreak.Beambreak;
+import frc.robot.subsystems.intake.roller.IntakeRoller;
+import frc.robot.subsystems.intake.roller.IntakeRoller.IntakeRollerGoal;
+
+public class OuttakeL1 extends Command {
+
+    private final Beambreak beambreak;
+    private final IntakeRoller roller;
+
+    public OuttakeL1(RobotContainer robot) {
+        beambreak = robot.getBeamBreak();
+        roller = robot.getIntakeRoller();
+
+        addRequirements(roller);
+    }
+
+    @Override
+    public void initialize() {
+        roller.setCurrentGoal(IntakeRollerGoal.OUTTAKECORRAL);
+    }
+
+    @Override
+    public void execute() {}
+
+    @Override
+    public boolean isFinished() {
+        return !beambreak.coralInIntake();
+    }
+
+    @Override
+    public void end(boolean isFinished) {
+        roller.setCurrentGoal(IntakeRollerGoal.IDLE);
+    }
+}

src/main/java/frc/robot/commands/intake/RunIntake.java

@@ -23,28 +23,26 @@ public RunIntake(RobotContainer robot) {
         beamBreak = robot.getBeamBreak();
         pivot = robot.getIntakePivot();
         addRequirements(roller, pivot);
-        timer = new Timer();  
+        timer = new Timer();
     }
 
     @Override
     public void initialize() {
         timer.reset();
-        roller.setGoal(IntakeRollerGoal.INTAKE);
+        roller.setCurrentGoal(IntakeRollerGoal.INTAKE);
         pivot.runPosition(IntakePivotGoal.TO_INTAKE);
     }
-    
+
     @Override
     public void execute() {}
-    
+
     @Override
     public boolean isFinished() {
-        return beamBreak.coralInIntake()
-                    || timer.hasElapsed(kTimeOutTime);
-
+        return beamBreak.coralInIntake() || timer.hasElapsed(kTimeOutTime);
     }
-    
+
     @Override
     public void end(boolean isFinished) {
-        roller.setGoal(IntakeRollerGoal.IDLE);
+        roller.setCurrentGoal(IntakeRollerGoal.IDLE);
     }
 }

src/main/java/frc/robot/commands/intake/intakepivot/IntakePivotToPosition.java

@@ -28,7 +28,7 @@ public void execute() {
 
     @Override
     public boolean isFinished() {
-        return intake.atGoal();
+        return intake.getCurrentGoal() == goal.get() && intake.atGoal();
     }
 
     @Override

src/main/java/frc/robot/commands/intake/intakeroller/OuttakeCoral.java

@@ -19,19 +19,19 @@ public OuttakeCoral(RobotContainer robot) {
 
     @Override
     public void initialize() {
-        roller.setGoal(IntakeRollerGoal.OUTTAKECORRAL);
+        roller.setCurrentGoal(IntakeRollerGoal.OUTTAKECORRAL);
     }
-    
+
     @Override
     public void execute() {}
-    
+
     @Override
     public boolean isFinished() {
         return !beamBreak.coralInIntake();
     }
-    
+
     @Override
     public void end(boolean isFinished) {
-        roller.setGoal(IntakeRollerGoal.IDLE);
+        roller.setCurrentGoal(IntakeRollerGoal.IDLE);
     }
 }

src/main/java/frc/robot/subsystems/intake/roller/IntakeRoller.java

@@ -1,11 +1,13 @@
 package frc.robot.subsystems.intake.roller;
 
-import static frc.robot.subsystems.elevator.ElevatorConstants.*;
+import static frc.robot.subsystems.intake.roller.IntakeRollerConstants.*;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.lib.LoggedTunableNumber;
+import frc.robot.lib.util.EqualsUtil;
 import java.util.function.DoubleSupplier;
 import lombok.Getter;
+import lombok.Setter;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
@@ -41,16 +43,14 @@ private IntakeRollerGoal(DoubleSupplier rollerVel) {
         }
     }
 
-    IntakeRollerGoal currentGoal = IntakeRollerGoal.IDLE;
+    @Getter
+    @Setter
+    private IntakeRollerGoal currentGoal = IntakeRollerGoal.IDLE;
 
     public IntakeRoller(IntakeRollerIO io) {
         this.io = io;
     }
 
-    public void setGoal(IntakeRollerGoal goal) {
-        currentGoal = goal;
-    }
-
     @Override
     public void periodic() {
         io.updateInputs(inputs);
@@ -77,6 +77,9 @@ public void periodic() {
      */
     @AutoLogOutput(key = "Intake Roller/At Goal")
     public boolean atGoal() {
-        return false;
+        return EqualsUtil.epsilonEquals(
+                (inputs.motorVelocityRPS[0] + inputs.motorVelocityRPS[1]) / 2,
+                currentGoal.getRollerVel().getAsDouble(),
+                kVelocityTolerance);
     }
 }

src/main/java/frc/robot/subsystems/outtake/wrist/Wrist.java

@@ -7,7 +7,6 @@
 import frc.robot.lib.util.EqualsUtil;
 import frc.robot.subsystems.elevator.Elevator.ElevatorGoal;
 import java.util.function.DoubleSupplier;
-
 import lombok.Getter;
 import lombok.Setter;
 import org.littletonrobotics.junction.AutoLogOutput;