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..7bb2914 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.kScore), + 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))) };