fix PP sim auto

allow it to flip itself and move from its starting position
FIXME: Running a path following auto on red alliance and then running PPSimpleAuto right after causes the auto to go in the wrong direction.

src/main/java/org/carlmontrobotics/RobotContainer.java

@@ -88,7 +88,7 @@ public class RobotContainer {
     // the last auto is hard-coded to go straight. since we have __3__ Autos, port 4
     // is simple
     // straight
-    private List<Command> autoCommands = new ArrayList<Command>();
+    private List<Command> autoCommands;
     private SendableChooser<Integer> autoSelector = new SendableChooser<Integer>();
 
     private boolean hasSetupAutos = false;
@@ -407,68 +407,73 @@ private void registerAutoCommands() {
 
     private void setupAutos() {
         //// CREATING PATHS from files
-        {
+        if (!hasSetupAutos) {
+            autoCommands=new ArrayList<Command>();//clear old/nonexistent autos
+
             for (int i = 0; i < autoNames.length; i++) {
                 String name = autoNames[i];
 
                 autoCommands.add(new PathPlannerAuto(name));
 
                 /*
-                 * // TODO: Charles' opinion: we shouldn't 7689[n' hhave it path find to the
-                 * starting pose
-                 * at the start of match new SequentialCommandGroup( AutoBuilder.pathfindToPose(
-                 * //
-                 * PathPlannerAuto.getStaringPoseFromAutoFile(name),
-                 * PathPlannerAuto.getPathGroupFromAutoFile(name).get(0).
-                 * getPreviewStartingHolonomicPose(),
-                 * Autoc.pathConstraints ), new PathPlannerAuto(name) );
+                 * // Charles' opinion: we shouldn't have it path find to the starting pose at the start of match
+                 * new SequentialCommandGroup( 
+                 *      AutoBuilder.pathfindToPose(
+                 *          PathPlannerAuto.getStaringPoseFromAutoFile(name),
+                 *          PathPlannerAuto.getPathGroupFromAutoFile(name).get(0).
+                 *          getPreviewStartingHolonomicPose(),
+                 *          Autoc.pathConstraints), 
+                 *      new PathPlannerAuto(name));
                  */
             }
-        }
-
-        // AUTOGENERATED AUTO FOR SLOT 2
-        {
-            Pose2d currPos = drivetrain.getPose();
-            // Create a list of bezier points from poses. Each pose represents one waypoint.
-            // The rotation component of the pose should be the direction of travel. Do not
-            // use holonomic
-            // rotation.
-            List<Translation2d> bezierPoints = PathPlannerPath.bezierFromPoses(
-                    currPos, currPos.plus(new Transform2d(0, 1, new Rotation2d(0))));
-            /**
-             * PATHPLANNER SETTINGS Robot Width (m): .91 Robot Length(m): .94 Max Module Spd
-             * (m/s): 4.30
-             * Default Constraints Max Vel: 1.54, Max Accel: 6.86 Max Angvel: 360, Max
-             * AngAccel: 360
-             * (guesses!)
-             */
-            // Create the path using the bezier points created above
-            PathPlannerPath path = new PathPlannerPath(bezierPoints,
-                    /* m/s, m/s^2, rad/s, rad/s^2 */
-                    Autoc.pathConstraints, new GoalEndState(0, currPos.getRotation()));
-            // Prevent the path from being flipped if the coordinates are already correct
-            path.preventFlipping = DriverStation.getAlliance().get() == DriverStation.Alliance.Blue;
-            if (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) {
-                path.flipPath();
-            }
+            hasSetupAutos = true;
 
             // NOTHING
             autoCommands.add(0, new PrintCommand("Running NULL Auto!"));
             // RAW FORWARD command
             autoCommands.add(1, new LastResortAuto(drivetrain));
-            // smart forward command
-            autoCommands.add(2,
-                    new SequentialCommandGroup(AutoBuilder.followPath(path)));
-            // no events so just use path instead of auto
-
+            // dumb PP forward command
+            autoCommands.add(2, new PrintCommand("PPSimpleAuto not Configured!"));
         }
+        // force regeneration each auto call
+        autoCommands.set(2, constructPPSimpleAuto());//overwrite this slot each time auto runs
+    }
+
+    public Command constructPPSimpleAuto() {
+        /**
+         * PATHPLANNER SETTINGS Robot Width (m): .91 Robot Length(m): .94 Max Module Spd
+         * (m/s): 4.30
+         * Default Constraints Max Vel: 1.54, Max Accel: 6.86 Max Angvel: 360, Max
+         * AngAccel: 360
+         * (guesses!)
+         */
+        // default origin is on BLUE ALIANCE DRIVER RIGHT CORNER
+        Pose2d currPos = drivetrain.getPose(); 
+
+        //FIXME running red PP file autos seems to break something, so the robot drivetrain drives in the wrong direction.
+            //running blue PP autos is fine though
+        //Note: alliance detection and path generation work correctly!
+        //Solution: Redeploy after auto.
+        Pose2d endPos = (DriverStation.getAlliance().get() == DriverStation.Alliance.Blue)
+                ? currPos.transformBy(new Transform2d(1, 0, new Rotation2d(0)))
+                : currPos.transformBy(new Transform2d(-1, 0, new Rotation2d(0)));
+
+        List<Translation2d> bezierPoints = PathPlannerPath.bezierFromPoses(currPos, endPos);
+
+        // Create the path using the bezier points created above, /* m/s, m/s^2, rad/s, rad/s^2 */
+        PathPlannerPath path = new PathPlannerPath(bezierPoints,
+                Autoc.pathConstraints, new GoalEndState(0, currPos.getRotation()));
+
+        path.preventFlipping = true;// don't flip, we do that manually already.
+
+        return new SequentialCommandGroup(
+            new InstantCommand(()->drivetrain.drive(-.0001, 0, 0)),//align drivetrain wheels.
+            AutoBuilder.followPath(path).beforeStarting(new WaitCommand(1)));
     }
 
     public Command getAutonomousCommand() {
-        if (!hasSetupAutos) {
-            setupAutos();
-            hasSetupAutos = true;
-        }
+        setupAutos();
+
         Integer autoIndex = autoSelector.getSelected();
 
         if (autoIndex != null && autoIndex != 0) {