diff --git a/src/main/java/frc/robot/subsystems/Bass.java b/src/main/java/frc/robot/subsystems/Bass.java index c8a7a1b..45920b6 100644 --- a/src/main/java/frc/robot/subsystems/Bass.java +++ b/src/main/java/frc/robot/subsystems/Bass.java @@ -2,6 +2,9 @@ import org.littletonrobotics.junction.LoggedRobot; +import java.util.List; + + import java.lang.annotation.Target; import java.util.Optional; @@ -14,6 +17,9 @@ import com.pathplanner.lib.util.PathPlannerLogging; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.wpilibj.DriverStation; @@ -23,6 +29,7 @@ import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -104,6 +111,33 @@ public Bass() { this // Reference to this subsystem to set requirements ); PathPlannerLogging.setLogCurrentPoseCallback(pose -> Logger.recordOutput("Chassis/targetPose",pose)); + + // 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( + new Pose2d(1.0, 1.0, Rotation2d.fromDegrees(0)), + new Pose2d(3.0, 1.0, Rotation2d.fromDegrees(0)), + new Pose2d(5.0, 3.0, Rotation2d.fromDegrees(90))); + + // Create the path using the bezier points created above + PathPlannerPath path = new PathPlannerPath( + bezierPoints, + new PathConstraints(3.0, 3.0, 2 * Math.PI, 4 * Math.PI), + // The constraints for this path. If using a differential drivetrain, the angularmconstraints have no effect. + + new GoalEndState(0.0, Rotation2d.fromDegrees(-90)) + // Goal end state. You can set a holonomic rotation here. If using a differential drivetrain, the rotation will have no effect. + ); + + // Prevent the path from being flipped if the coordinates are already correct + path.preventFlipping = false; + } + + public Command alignToSpeaker(){ + return runOnce(( -> { + + })); } @Override