From a0e70a2f8f9e88fe90d20843932e52907e927eeb Mon Sep 17 00:00:00 2001 From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com> Date: Sun, 25 Aug 2024 15:21:34 -0700 Subject: [PATCH] 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. --- .../org/carlmontrobotics/RobotContainer.java | 97 ++++++++++--------- 1 file changed, 51 insertions(+), 46 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 84026de..a46f8ec 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/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 autoCommands = new ArrayList(); + private List autoCommands; private SendableChooser autoSelector = new SendableChooser(); private boolean hasSetupAutos = false; @@ -407,68 +407,73 @@ private void registerAutoCommands() { private void setupAutos() { //// CREATING PATHS from files - { + if (!hasSetupAutos) { + autoCommands=new ArrayList();//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 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 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) {