some bug fixes regarding pp auto

Shenzhen-Robotics-Alliance · Oct 18, 2024 · 0ffc088 · 0ffc088
1 parent bf6d0f8
commit 0ffc088
Show file tree

Hide file tree

Showing 6 changed files with 28 additions and 28 deletions.
diff --git a/.../main/java/frc/robot/subsystems/vision/apriltags/CameraHeightAndPitchRollAngleFilter.java b/.../main/java/frc/robot/subsystems/vision/apriltags/CameraHeightAndPitchRollAngleFilter.java
@@ -4,7 +4,7 @@
 
 
 public class CameraHeightAndPitchRollAngleFilter implements VisionResultsFilter  {
-    private static final double ROBOT_HEIGHT_TOLERANCE = 0.12, ROBOT_PITCH_TOLERANCE_RADIANS = Math.toRadians(5), ROBOT_ROLL_TOLERANCE_RADIANS = Math.toRadians(5);
+    private static final double ROBOT_HEIGHT_TOLERANCE = 0.5, ROBOT_PITCH_TOLERANCE_RADIANS = Math.toRadians(12), ROBOT_ROLL_TOLERANCE_RADIANS = Math.toRadians(12);
 
     @Override
     public String getFilterImplementationName() {

diff --git a/src/main/deploy/pathplanner/paths/rush first pp.path b/src/main/deploy/pathplanner/paths/rush first pp.path
@@ -36,8 +36,8 @@
         "y": 7.478931968540093
       },
       "prevControl": {
-        "x": 7.783346907144593,
-        "y": 7.711056946431803
+        "x": 7.783106474914461,
+        "y": 7.7111611478207625
       },
       "nextControl": {
         "x": 8.77771550807551,
@@ -61,6 +61,10 @@
     }
   ],
   "rotationTargets": [
+    {
+      "waypointRelativePos": 1.5612788632326908,
+      "rotationDegrees": 180.0
+    },
     {
       "waypointRelativePos": 2,
       "rotationDegrees": 161.565051177078

diff --git a/src/main/deploy/pathplanner/paths/rush third pp.path b/src/main/deploy/pathplanner/paths/rush third pp.path
@@ -16,16 +16,16 @@
     },
     {
       "anchor": {
-        "x": 6.390785355710689,
-        "y": 4.401663085651201
+        "x": 6.538805884254711,
+        "y": 4.128993690964843
       },
       "prevControl": {
-        "x": 5.755026402877054,
-        "y": 4.261495757467408
+        "x": 5.903046931421077,
+        "y": 3.98882636278105
       },
       "nextControl": {
-        "x": 7.627535824466668,
-        "y": 4.674332480337558
+        "x": 7.775556353010691,
+        "y": 4.4016630856512
       },
       "isLocked": false,
       "linkedName": null
@@ -79,7 +79,7 @@
   "rotationTargets": [
     {
       "waypointRelativePos": 2,
-      "rotationDegrees": 147.6926655451179
+      "rotationDegrees": 162.07967008819475
     }
   ],
   "constraintZones": [],

diff --git a/src/main/java/frc/robot/autos/ExampleCustomAutoWithPathPlannerTrajectories.java b/src/main/java/frc/robot/autos/ExampleCustomAutoWithPathPlannerTrajectories.java
@@ -1,9 +1,11 @@
 package frc.robot.autos;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathPlannerPath;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.RobotContainer;
 import org.json.simple.parser.ParseException;
 
@@ -12,11 +14,18 @@
 public class ExampleCustomAutoWithPathPlannerTrajectories implements Auto {
     @Override
     public Command getAutoCommand(RobotContainer robot) throws IOException, ParseException {
-        return Commands.none();
+        final SequentialCommandGroup commandGroup = new SequentialCommandGroup();
+
+        commandGroup.addCommands(AutoBuilder.followPath(PathPlannerPath.fromPathFile("rush first pp")));
+        commandGroup.addCommands(AutoBuilder.followPath(PathPlannerPath.fromPathFile("rush second pp")));
+        commandGroup.addCommands(AutoBuilder.followPath(PathPlannerPath.fromPathFile("rush third pp")));
+        commandGroup.addCommands(AutoBuilder.followPath(PathPlannerPath.fromPathFile("rush fourth and shoot pp")));
+
+        return commandGroup;
     }
 
     @Override
     public Pose2d getStartingPoseAtBlueAlliance() {
-        return new Pose2d(1.3, 7.2, Rotation2d.fromDegrees(180));
+        return new Pose2d(1.49, 7.65, Rotation2d.fromDegrees(180));
     }
 }
diff --git a/src/main/java/frc/robot/autos/PathPlannerAutoWrapper.java b/src/main/java/frc/robot/autos/PathPlannerAutoWrapper.java
@@ -12,8 +12,7 @@ public class PathPlannerAutoWrapper implements Auto {
 
     public PathPlannerAutoWrapper(String name) {
         this.name = name;
-        final PathPlannerAuto auto = new PathPlannerAuto(name);
-        this.startingPose = auto.getStartingPose();
+        this.startingPose = new PathPlannerAuto(name).getStartingPose();
     }
 
     @Override

diff --git a/src/main/java/frc/robot/subsystems/drive/HolonomicDriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/HolonomicDriveSubsystem.java
@@ -37,15 +37,6 @@ public interface HolonomicDriveSubsystem extends Subsystem {
      * */
     void runRawChassisSpeeds(ChassisSpeeds speeds);
 
-    /**
-     * runs a ChassisSpeeds and a set of motor feedforwards without doing any pre-processing
-     * @param speeds a discrete chassis speed, robot-centric
-     * @param driveFeedforwards a set of {@link DriveFeedforward} to run on the drive motors, on default it is ignored
-     * */
-    default void runRawChassisSpeeds(ChassisSpeeds speeds, DriveFeedforward[] driveFeedforwards) {
-        runRawChassisSpeeds(speeds);
-    }
-
     /**
      * Returns the current odometry Pose.
      */
@@ -137,10 +128,7 @@ default void configHolonomicPathPlannerAutoBuilder() {
                 this::getPose,
                 this::setPose,
                 this::getMeasuredChassisSpeedsRobotRelative,
-                (speeds, feedForwards) -> this.runRawChassisSpeeds(
-                        ChassisSpeeds.discretize(speeds, Robot.defaultPeriodSecs),
-                        feedForwards
-                ),
+                this::runRawChassisSpeeds,
                 new PPHolonomicDriveController(CHASSIS_TRANSLATION_CLOSE_LOOP.toPathPlannerPIDConstants(), CHASSIS_ROTATION_CLOSE_LOOP.toPathPlannerPIDConstants()),
                 new RobotConfig(
                         DriveTrainConstants.ROBOT_MASS_KG,