2piece crauton

src/main/java/frc/robot/Constants.java

@@ -25,7 +25,7 @@
  * wherever the constants are needed, to reduce verbosity.
  */
 public final class Constants {
-    public static final boolean IS_R1 = false;
+    public static final boolean IS_R1 = true;
     public static final boolean GLOBAL_SHUFFLEBOARD_ENABLE = false;
 
     public static final class TankConstants {

src/main/java/frc/robot/RobotContainer.java

@@ -24,7 +24,8 @@
 import frc.robot.commands.auton.BalanceAutonSequence;
 import frc.robot.commands.auton.BottomBalanceAutonSequence;
 import frc.robot.commands.auton.BottomOnePieceAutonSequence;
-import frc.robot.commands.auton.BottomTwoPieceAutonSequence;
+import frc.robot.commands.auton.EarlyTurnBottomTwoPieceAutonSequence;
+import frc.robot.commands.auton.OgBottomTwoPieceAutonSequence;
 import frc.robot.commands.auton.PreloadedOnlyAutonSequence;
 import frc.robot.commands.auton.TopOnePieceAutonSequence;
 import frc.robot.commands.auton.TopTwoPieceAutonSequence;

src/main/java/frc/robot/commands/auton/EarlyTurnBottomTwoPieceAutonSequence.java

@@ -0,0 +1,62 @@
+package frc.robot.commands.auton;
+
+import java.util.List;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+import frc.robot.commands.swerve.FollowPathCommand;
+import frc.robot.positions.FieldPosition;
+import frc.robot.positions.PlacePosition;
+import frc.robot.subsystems.RollerSubsystem;
+import frc.robot.subsystems.drivetrain.BaseSwerveSubsystem;
+import frc.robot.subsystems.tiltedelevator.ElevatorState;
+import frc.robot.subsystems.tiltedelevator.TiltedElevatorSubsystem;
+
+public class EarlyTurnBottomTwoPieceAutonSequence extends BaseAutonSequence {
+    private static final PlacePosition INITIAL_POSE = PlacePosition.A2_HIGH;
+
+    private static final FieldPosition MID_POSE_1 = FieldPosition.BOTTOM_MIDPOS_1;
+    private static final FieldPosition TEST_POS = FieldPosition.BOTTOM_EARLYTURNTEST_MIDPOS;
+    private static final FieldPosition MID_POSE_2 = FieldPosition.BOTTOM_MIDPOS_2;
+    private static final FieldPosition MID_POSE_3 = FieldPosition.BOTTOM_MIDPOS_3;
+
+    private static final FieldPosition GRAB_POSE = FieldPosition.PIECE4;
+    private static final PlacePosition PLACE_POSE2 = PlacePosition.A2_MID;
+
+    /**
+     * Non-balancing bottom auton sequence.
+     * Turns early
+     * @param swerveSubsystem The swerve subsystem.
+     * @param rollerSubsystem The roller subsystem.
+     * @param tiltedElevatorSubsystem The tilted elevator subsystem.
+     * @param isRed Whether this is a red auton path.
+     */
+    public EarlyTurnBottomTwoPieceAutonSequence(
+        BaseSwerveSubsystem swerveSubsystem, RollerSubsystem rollerSubsystem, TiltedElevatorSubsystem tiltedElevatorSubsystem,
+        boolean isRed // TODO: better way of passing this
+    ) {
+        super(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, INITIAL_POSE, isRed); // TODO: better
+
+        Pose2d initialPose = INITIAL_POSE.alignPosition.getPose(isRed);
+
+        Pose2d midPose1 = MID_POSE_1.getPose(isRed);
+        Pose2d testPose = TEST_POS.getPose(isRed);
+        Pose2d midPose2 = MID_POSE_2.getPose(isRed);
+        Pose2d midPose3 = MID_POSE_3.getPose(isRed);
+
+        Pose2d grabPose = GRAB_POSE.getPose(isRed);
+
+        Pose2d placePose2 = PLACE_POSE2.placePosition.getPose(isRed);
+        ElevatorState elevatorState = PLACE_POSE2.elevatorState;
+
+        addCommands(
+            //Start 180 turn at testPose
+            FollowPathCommand.from(swerveSubsystem, initialPose, List.of(midPose1.getTranslation()), testPose, false, true),
+            FollowPathCommand.from(swerveSubsystem, testPose, List.of(midPose2.getTranslation()), midPose3, true, true),
+            // Go and grab 2nd piece
+            goAndGrab(midPose3, List.of(), grabPose, true, false),
+            // Go and place grabbed piece
+            goAndPlace(grabPose, List.of(midPose3,  testPose), midPose1, placePose2, elevatorState) // skips past midPose2 right now, so turning has a larger frame of time....
+        );
+    }
+}

src/main/java/frc/robot/commands/auton/BottomTwoPieceAutonSequence.java → src/main/java/frc/robot/commands/auton/OgBottomTwoPieceAutonSequence.java

@@ -12,43 +12,51 @@
 import frc.robot.subsystems.tiltedelevator.ElevatorState;
 import frc.robot.subsystems.tiltedelevator.TiltedElevatorSubsystem;
 
-public class BottomTwoPieceAutonSequence extends BaseAutonSequence {
+public class OgBottomTwoPieceAutonSequence extends BaseAutonSequence {
+    private static final PlacePosition INITIAL_POSE = PlacePosition.A2_HIGH;
+
     private static final FieldPosition MID_POSE_1 = FieldPosition.BOTTOM_MIDPOS_1;
     private static final FieldPosition MID_POSE_2 = FieldPosition.BOTTOM_MIDPOS_2;
+    private static final FieldPosition MID_POSE_3 = FieldPosition.BOTTOM_MIDPOS_3;
 
     private static final FieldPosition GRAB_POSE = FieldPosition.PIECE4;
     private static final PlacePosition PLACE_POSE2 = PlacePosition.A2_MID;
 
     /**
      * Non-balancing bottom auton sequence.
+     * Turns early
      * @param swerveSubsystem The swerve subsystem.
      * @param rollerSubsystem The roller subsystem.
      * @param tiltedElevatorSubsystem The tilted elevator subsystem.
-     * @param initialPosition The initial place position of the sequence.
      * @param isRed Whether this is a red auton path.
      */
-    public BottomTwoPieceAutonSequence(
+    public OgBottomTwoPieceAutonSequence(
         BaseSwerveSubsystem swerveSubsystem, RollerSubsystem rollerSubsystem, TiltedElevatorSubsystem tiltedElevatorSubsystem,
-        PlacePosition initialPosition, boolean isRed // TODO: better way of passing this
+        boolean isRed // TODO: better way of passing this
     ) {
-        super(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, initialPosition, isRed);
+        super(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, INITIAL_POSE, isRed);
 
-        Pose2d initialPose = initialPosition.alignPosition.getPose(isRed);
+        Pose2d initialPose = INITIAL_POSE.alignPosition.getPose(isRed);
         Pose2d midPose1 = MID_POSE_1.getPose(isRed);
         Pose2d midPose2 = MID_POSE_2.getPose(isRed);
+        Pose2d midPose3 = MID_POSE_3.getPose(isRed);
 
         Pose2d grabPose = GRAB_POSE.getPose(isRed);
 
         Pose2d placePose2 = PLACE_POSE2.placePosition.getPose(isRed);
         ElevatorState elevatorState2 = PLACE_POSE2.elevatorState;
 
         addCommands(
-            // Spline through midpose 1 to avoid overshoot
+            // Spline through midPose 1 to avoid overshoot
             FollowPathCommand.from(swerveSubsystem, initialPose, List.of(midPose1.getTranslation()), midPose2, false, true),
+            FollowPathCommand.from(swerveSubsystem, midPose2, List.of(), midPose3, true, true),
             // Go and grab 2nd piece
-            goAndGrab(midPose2, List.of(), grabPose, true, false),
+            goAndGrab(midPose3, List.of(), grabPose, true, false),
             // Go and place grabbed piece
-            goAndPlace(grabPose, List.of(midPose2), midPose1, placePose2, elevatorState2)
+            FollowPathCommand.from(swerveSubsystem, grabPose, List.of(), midPose2, false, true),
+            FollowPathCommand.from(swerveSubsystem, midPose2, List.of(), midPose1, true, true),
+            //goAndPlace(midPose2, List.of(midPose1), initialPose, placeState2)
+            goAndPlace(midPose1, placePose2, elevatorState2, true)
         );
     }
 }

src/main/java/frc/robot/commands/swerve/TurnCommand.java

@@ -0,0 +1,17 @@
+package frc.robot.commands.swerve;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.drivetrain.BaseSwerveSubsystem;
+import frc.robot.subsystems.drivetrain.SwerveSubsystem;
+
+public class TurnCommand extends CommandBase{
+    private final BaseSwerveSubsystem swerveSubsystem;
+    private final Rotation2d targetRotation;
+
+    public TurnCommand(BaseSwerveSubsystem swerveSubsystem, Rotation2d targetRotation){
+        this.swerveSubsystem = swerveSubsystem;
+        this.targetRotation = targetRotation;
+    }
+
+}

src/main/java/frc/robot/positions/FieldPosition.java

@@ -109,11 +109,23 @@ public enum FieldPosition {
         Units.inchesToMeters(29),
         Rotation2d.fromDegrees(180)
     )),
+
+    BOTTOM_EARLYTURNTEST_MIDPOS(new Pose2d( // pose to hit before starting the early turning, to prevent robot from swinging thru midPose1
+        Units.inchesToMeters(126.635),
+        Units.inchesToMeters(29),
+        Rotation2d.fromDegrees(180)
+    )),
+
     BOTTOM_MIDPOS_2(new Pose2d(
         Units.inchesToMeters(170),
         Units.inchesToMeters(29), 
         Rotation2d.fromDegrees(180)
     )),
+    BOTTOM_MIDPOS_3(new Pose2d(
+        Units.inchesToMeters(246.543),
+        Units.inchesToMeters(34.97), 
+        Rotation2d.fromDegrees(0)
+    )),
 
     BOTTOM_1PIECEPOS(new Pose2d(
         Units.inchesToMeters(238.152),

src/main/java/frc/robot/subsystems/tiltedelevator/ElevatorState.java

@@ -11,7 +11,7 @@ public enum ElevatorState {
     CUBE_MID(Units.inchesToMeters(Constants.IS_R1 ? 33 : 40)), // absolute height = 14.25 in
     CUBE_HIGH(Units.inchesToMeters(Constants.IS_R1 ? 53 : 55)), // absolute height = 31.625 in
     CONE_MID(Units.inchesToMeters(Constants.IS_R1 ? 50 : 48), 0, -Units.inchesToMeters(10)), // absolute height = 34 in
-    CONE_HIGH(Units.inchesToMeters(Constants.IS_R1 ? 53 : 61)), // absolute height = 46 in
+    CONE_HIGH(Units.inchesToMeters(Constants.IS_R1 ? 55 : 61)), // absolute height = 46 in
     HOME(Units.inchesToMeters(0));
 
     private final double extendDistanceMeters; // meters, extension distance of winch

src/test/java/AutonPathTest.java

@@ -4,7 +4,8 @@
 import frc.robot.commands.auton.BalanceAutonSequence;
 import frc.robot.commands.auton.BottomBalanceAutonSequence;
 import frc.robot.commands.auton.BottomOnePieceAutonSequence;
-import frc.robot.commands.auton.BottomTwoPieceAutonSequence;
+import frc.robot.commands.auton.EarlyTurnBottomTwoPieceAutonSequence;
+import frc.robot.commands.auton.OgBottomTwoPieceAutonSequence;
 import frc.robot.commands.auton.PreloadedOnlyAutonSequence;
 import frc.robot.commands.auton.TopOnePieceAutonSequence;
 import frc.robot.commands.auton.TopTwoPieceAutonSequence;
@@ -50,7 +51,8 @@ public void compileRedPaths() {
         BalanceAutonSequence.withDeadline(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, PlacePosition.B2_HIGH, true);
         BalanceAndTaxiAutonSequence.withDeadline(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, PlacePosition.B2_HIGH, true);
         new BottomOnePieceAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, PlacePosition.A2_HIGH, true);
-        new BottomTwoPieceAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, PlacePosition.A2_HIGH, true);
+        new EarlyTurnBottomTwoPieceAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, true);
+        new OgBottomTwoPieceAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, true);
         BottomBalanceAutonSequence.withDeadline(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, PlacePosition.A2_HIGH, true);
     }
 
@@ -65,7 +67,8 @@ public void compileBluePaths() {
         BalanceAutonSequence.withDeadline(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, PlacePosition.B2_HIGH, false);
         BalanceAndTaxiAutonSequence.withDeadline(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, PlacePosition.B2_HIGH, false);
         new BottomOnePieceAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, PlacePosition.A2_HIGH, false);
-        new BottomTwoPieceAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, PlacePosition.A2_HIGH, false);
+        new EarlyTurnBottomTwoPieceAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, false);
+        new OgBottomTwoPieceAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, false);
         BottomBalanceAutonSequence.withDeadline(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, PlacePosition.A2_HIGH, false);
     }
 }