COMMIT STUFF

Author: ShmayaR

src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java

@@ -49,7 +49,7 @@ public static Command getFloorAlgaeCollectionCommand() {
         return new SequentialCommandGroup(
                 GeneralCommands.getResetFlipArmOverrideCommand(),
                 CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.END_EFFECTOR::hasGamePiece),
-                getInitiateFloorAlgaeCollectionCommand().raceWith(new WaitUntilCommand(RobotContainer.END_EFFECTOR::hasGamePiece).andThen(new WaitCommand(0.15))),
+                getInitiateFloorAlgaeCollectionCommand().raceWith(new WaitUntilCommand(RobotContainer.END_EFFECTOR::hasGamePiece).andThen(new WaitCommand(0.1))),
                 new InstantCommand(() -> {
                     IS_HOLDING_ALGAE = true;
                     SHOULD_COLLECT_FROM_LOLLIPOP = false;

src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java

@@ -2,6 +2,7 @@
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.trigon.robot.RobotContainer;
 import frc.trigon.robot.commands.commandfactories.GeneralCommands;
@@ -40,18 +41,19 @@ public static Command getArmGearRatioCalulationCommand() {
     }
 
     public static Command resetElevatorPositionCommand() {
-        return new SequentialCommandGroup(
+        return new ParallelCommandGroup(
                 new ExecuteEndCommand(
-                        () -> RobotContainer.ARM_ELEVATOR.setTargetState(ArmElevatorConstants.ArmElevatorState.ZERO_ELEVATOR),
-                        RobotContainer.ARM_ELEVATOR::stop,
-                        RobotContainer.ARM_ELEVATOR
+                        () -> RobotContainer.ARM_ELEVATOR.setTargetArmState(ArmElevatorConstants.ArmElevatorState.ZERO_ELEVATOR, false),
+                        () -> {}
                 ).until(() -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.ZERO_ELEVATOR)),
                 new ExecuteEndCommand(
                         () -> RobotContainer.ARM_ELEVATOR.setElevatorVoltage(OperatorConstants.DRIVER_CONTROLLER.getRightY() * 2),
                         () -> {},
                         RobotContainer.ARM_ELEVATOR
-                )
-        ).alongWith(SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0), EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.EJECT)).finallyDo(RobotContainer.ARM_ELEVATOR::resetElevatorPosition);
+                ),
+                SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0),
+                EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.EJECT)
+        ).finallyDo(() -> {RobotContainer.ARM_ELEVATOR.resetElevatorPosition(); RobotContainer.END_EFFECTOR.setTargetState(EndEffectorConstants.EndEffectorState.REST);});
     }
 
     public static Command getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState targetState) {

src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java

@@ -252,7 +252,7 @@ private static void configureElevatorMasterMotor() {
         config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
         config.Feedback.SensorToMechanismRatio = ELEVATOR_GEAR_RATIO;
 
-        config.Slot0.kP = RobotHardwareStats.isSimulation() ? 3.5 : 17;
+        config.Slot0.kP = RobotHardwareStats.isSimulation() ? 3.5 : 22;
         config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0;
         config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.4 : 1;
         config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016165 : 0.0546875;
@@ -339,13 +339,13 @@ public enum ArmElevatorState {
         SCORE_L1(Rotation2d.fromDegrees(-13), PREPARE_SCORE_L1.targetPositionMeters, null, false, 1),
         SCORE_L2(Rotation2d.fromDegrees(25), PREPARE_SCORE_L2.targetPositionMeters, PREPARE_SCORE_L2, false, 0.8),
         SCORE_L3(Rotation2d.fromDegrees(25), PREPARE_SCORE_L3.targetPositionMeters, PREPARE_SCORE_L3, false, 0.8),
-        SCORE_L4(Rotation2d.fromDegrees(-10), 1.5, PREPARE_SCORE_L4, false, 0.8),
+        SCORE_L4(Rotation2d.fromDegrees(-5), 1.5, PREPARE_SCORE_L4, false, 0.8),
         SCORE_NET(Rotation2d.fromDegrees(70), 1.61, null, false, 0.3),
-        SCORE_PROCESSOR(Rotation2d.fromDegrees(0), 0.1, null, false, 0.6),
+        SCORE_PROCESSOR(Rotation2d.fromDegrees(0), 0.1, null, false, 0.4),
         COLLECT_ALGAE_L2(Rotation2d.fromDegrees(0), 0.603, null, false, 1),
         COLLECT_ALGAE_L3(Rotation2d.fromDegrees(0), 1.15, null, false, 1),
-        PREPARE_COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-30), 0.3, null, false, 1),
-        COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-32), 0.16, PREPARE_COLLECT_ALGAE_FLOOR, true, 1),
+        PREPARE_COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-32), 0.3, null, false, 1),
+        COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-32), 0.18, PREPARE_COLLECT_ALGAE_FLOOR, true, 1),
         COLLECT_ALGAE_LOLLIPOP(Rotation2d.fromDegrees(0), 0.15, null, false, 1);
 
 

src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java

@@ -61,7 +61,7 @@ public boolean isEjecting() {
         return endEffectorMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE) > 1;
     }
 
-    void setTargetState(EndEffectorConstants.EndEffectorState targetState) {
+    public void setTargetState(EndEffectorConstants.EndEffectorState targetState) {
         if (targetState == EndEffectorConstants.EndEffectorState.HOLD_ALGAE) {
             setCur(targetState.targetVoltage);
         } else {