auton commands

.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"
 }

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) {

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<Double> mVelocitySupplier;

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)))
     };