From 96c7797a639965a3119d3e5a2179895508a17120 Mon Sep 17 00:00:00 2001 From: Pix_XL <82770059+PIXLwastaken@users.noreply.github.com> Date: Sat, 22 Mar 2025 00:14:02 -0400 Subject: [PATCH 1/2] just shoot it atp (auton commands) --- .vscode/settings.json | 3 +- .../frc/robot/commands/RobotCommands.java | 73 +++++++++++++++++++ .../frc/robot/elevator/ElevatorSubsystem.java | 2 +- .../java/frc/robot/vision/VisionConfig.java | 4 +- 4 files changed, 78 insertions(+), 4 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 612cdd0..1745ba0 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -56,5 +56,6 @@ "edu.wpi.first.math.proto.*", "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", - ] + ], + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" } diff --git a/src/main/java/frc/robot/commands/RobotCommands.java b/src/main/java/frc/robot/commands/RobotCommands.java index 4c91837..e1b6f58 100644 --- a/src/main/java/frc/robot/commands/RobotCommands.java +++ b/src/main/java/frc/robot/commands/RobotCommands.java @@ -10,6 +10,7 @@ import frc.robot.rushinator.RushinatorWrist; import frc.robot.rushinator.RushinatorWrist.State; import frc.robot.rushinator.commands.SetArmState; +import frc.robot.rushinator.commands.SetRollersVoltage; import frc.robot.rushinator.commands.SetWristState; public class RobotCommands { @@ -32,6 +33,78 @@ public static Command coralPrimeShoot(RushinatorPivot.State armState, Rushinator ); } + + public static Command scoreCoralAutonL4(){ + return new SequentialCommandGroup( + new ParallelRaceGroup( + new SetElevatorState(ElevatorSubsystem.State.kCoralL4), + new SetArmState(RushinatorPivot.State.kScore), + new SetWristState(RushinatorWrist.State.kScoreRightWrist), + new WaitUntilCommand(() -> ElevatorSubsystem.getInstance().mPPIDController.atGoal()) + ), + new ElevatorSubsystem.applyJog(ElevatorSubsystem.getInstance().getPosition() - 3.0), + new ParallelRaceGroup( + new SetArmState(RushinatorPivot.State.kStowTravel), + new SetWristState(RushinatorWrist.State.kTravelRight), + new WaitCommand(5) + ) + ); + } + + public static Command scoreCoralAutonL3(){ + return new SequentialCommandGroup( + new ParallelRaceGroup( + new SetElevatorState(ElevatorSubsystem.State.kCoralL3), + new SetArmState(RushinatorPivot.State.kScore), + new SetWristState(RushinatorWrist.State.kScoreRightWrist), + new WaitUntilCommand(() -> ElevatorSubsystem.getInstance().mPPIDController.atGoal()) + ), + new ElevatorSubsystem.applyJog(ElevatorSubsystem.getInstance().getPosition() - 3.0), + new ParallelRaceGroup( + new SetArmState(RushinatorPivot.State.kStowTravel), + new SetWristState(RushinatorWrist.State.kTravelRight), + new WaitCommand(5) + ) + ); + } + + public static Command scoreCoralAutonL2(){ + return new SequentialCommandGroup( + new ParallelRaceGroup( + new SetElevatorState(ElevatorSubsystem.State.kZero), + new SetArmState(RushinatorPivot.State.kScore), + new SetWristState(RushinatorWrist.State.kScoreRightWrist), + new WaitUntilCommand(() -> ElevatorSubsystem.getInstance().mPPIDController.atGoal()) + ), + new ElevatorSubsystem.applyJog(ElevatorSubsystem.getInstance().getPosition() - 3.0), + new ParallelRaceGroup( + new SetArmState(RushinatorPivot.State.kStowTravel), + new SetWristState(RushinatorWrist.State.kTravelRight), + new WaitCommand(5) + ) + ); + } + + public static Command scoreCoralAutonL1(){ + return new SequentialCommandGroup( + new ParallelRaceGroup( + new SetElevatorState(ElevatorSubsystem.State.kZero), + new SetArmState(RushinatorPivot.State.kScoreL1), + new SetWristState(RushinatorWrist.State.kScoreMid), + new WaitUntilCommand(() -> ElevatorSubsystem.getInstance().mPPIDController.atGoal()) + ), + new ParallelRaceGroup( + new SetRollersVoltage(3.5), + new WaitCommand(2) + ), + new ParallelRaceGroup( + new SetArmState(RushinatorPivot.State.kStowTravel), + new SetWristState(RushinatorWrist.State.kTravelRight), + new WaitCommand(5) + ) + ); + } + public static Command toggleWristState() { System.out.println("Last State in Toggle Wrist" + RushinatorWrist.kLastState.name()); if(RushinatorWrist.kLastState == State.kScoreLeftWrist) { diff --git a/src/main/java/frc/robot/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/elevator/ElevatorSubsystem.java index a9aa0af..09332c1 100644 --- a/src/main/java/frc/robot/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/elevator/ElevatorSubsystem.java @@ -50,7 +50,7 @@ public static class Settings { private TalonFX mTalonLeft, mTalonRight; private final ElevatorFeedforward mFFLowController, mFFHighController; - private final ProfiledPIDController mPPIDController; + public final ProfiledPIDController mPPIDController; private DigitalInput mLowerLimitSwitch; private Supplier mVelocitySupplier; diff --git a/src/main/java/frc/robot/vision/VisionConfig.java b/src/main/java/frc/robot/vision/VisionConfig.java index b36be35..68a6dc4 100644 --- a/src/main/java/frc/robot/vision/VisionConfig.java +++ b/src/main/java/frc/robot/vision/VisionConfig.java @@ -45,11 +45,11 @@ public class VisionConfig { public static final Transform3d[] ROBOT_TO_CAM_TRANSFORMS = new Transform3d[] { //left cam new Transform3d( - new Translation3d(Units.inchesToMeters(-12.436), Units.inchesToMeters(11.677), Units.inchesToMeters(7.413)), + new Translation3d(Units.inchesToMeters(12.436), Units.inchesToMeters(-11.677), Units.inchesToMeters(7.413)), new Rotation3d(0,Units.degreesToRadians(15),Units.degreesToRadians(-20))), //right cam new Transform3d( - new Translation3d(Units.inchesToMeters(-12.436), Units.inchesToMeters(-11.677), Units.inchesToMeters(7.413)), + new Translation3d(Units.inchesToMeters(12.436), Units.inchesToMeters(11.677), Units.inchesToMeters(7.413)), new Rotation3d(0, Units.degreesToRadians(15), Units.degreesToRadians(20))) }; From ea3726c2230e00782d2e22649fc133a4dfdc500e Mon Sep 17 00:00:00 2001 From: Pix_XL <82770059+PIXLwastaken@users.noreply.github.com> Date: Sat, 22 Mar 2025 00:15:51 -0400 Subject: [PATCH 2/2] fix dumbahh code --- src/main/java/frc/robot/commands/RobotCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/RobotCommands.java b/src/main/java/frc/robot/commands/RobotCommands.java index e1b6f58..7bb2914 100644 --- a/src/main/java/frc/robot/commands/RobotCommands.java +++ b/src/main/java/frc/robot/commands/RobotCommands.java @@ -89,7 +89,7 @@ public static Command scoreCoralAutonL1(){ return new SequentialCommandGroup( new ParallelRaceGroup( new SetElevatorState(ElevatorSubsystem.State.kZero), - new SetArmState(RushinatorPivot.State.kScoreL1), + new SetArmState(RushinatorPivot.State.kScore), new SetWristState(RushinatorWrist.State.kScoreMid), new WaitUntilCommand(() -> ElevatorSubsystem.getInstance().mPPIDController.atGoal()) ),