macros?

BitBucketsFRC4183 · Mar 7, 2025 · e5625de · e5625de
1 parent b30a4d9
commit e5625de
Show file tree

Hide file tree

Showing 2 changed files with 41 additions and 6 deletions.
diff --git a/src/main/java/frc/robot/commands/ArmElevatorToRest.java b/src/main/java/frc/robot/commands/ArmElevatorToRest.java
@@ -0,0 +1,31 @@
+package frc.robot.commands;
+
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
+import frc.robot.commands.ArmCommands.ArmBendCommand;
+import frc.robot.commands.ElevatorCommands.ElevatorSetPointCommand;
+import frc.robot.constants.ArmConstants;
+import frc.robot.constants.Constants;
+import frc.robot.subsystems.ArmSubsystem.ArmSubsystem;
+import frc.robot.subsystems.ElevatorSubsystem.ElevatorSubsystem;
+
+public class ArmElevatorToRest extends ParallelDeadlineGroup {
+    public ArmElevatorToRest(ElevatorSubsystem elevator, ArmSubsystem arm) {
+        super(Commands.deadline(
+                Commands.waitSeconds(Constants.commandTimeout + 1),
+                Commands.sequence(
+                        Commands.deadline(
+                                Commands.waitSeconds(0.2),
+                                new ArmBendCommand(arm, ArmConstants.MIN_ANGLE_RADS + Units.degreesToRadians(5)),
+                        Commands.parallel(
+                                new ArmBendCommand(arm, ArmConstants.MIN_ANGLE_RADS + Units.degreesToRadians(5)),
+                                new ElevatorSetPointCommand(elevator, -0.05))
+
+                )
+                )
+                )
+
+        );
+    }
+}
diff --git a/src/main/java/frc/robot/commands/ArmElevatorToSetpoint.java b/src/main/java/frc/robot/commands/ArmElevatorToSetpoint.java
@@ -12,12 +12,16 @@
 public class ArmElevatorToSetpoint extends ParallelDeadlineGroup {
     public ArmElevatorToSetpoint(ElevatorSubsystem elevator, ArmSubsystem arm, double elevatorSetpoint, double armSetpoint) {
         super(Commands.deadline(
-                Commands.waitSeconds(Constants.commandTimeout),
-                Commands.parallel(
-                        new ElevatorSetPointCommand(elevator, elevatorSetpoint),
-                        new ArmBendCommand(arm, armSetpoint)
+                Commands.waitSeconds(Constants.commandTimeout + 1),
+                Commands.sequence(
+                        Commands.deadline(
+                                Commands.waitSeconds(0.1),
+                        new ElevatorSetPointCommand(elevator, elevatorSetpoint)),
+                        Commands.parallel(
+                                new ArmBendCommand(arm, armSetpoint),
+                                new ElevatorSetPointCommand(elevator, elevatorSetpoint))
+                                )
                 )
-
-        ));
+        );
     }
 }