diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a44a6c8..a3a00f0 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -4,9 +4,12 @@ package frc4388.robot; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; 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.trajectory.TrapezoidProfile; import frc4388.utility.Gains; import frc4388.utility.LEDPatterns; @@ -61,6 +64,12 @@ public final class Constants { public static final int SWERVE_TIMEOUT_MS = 30; public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 1.0, 0.0, 0, 1.0); + // swerve auto constants + public static final PIDController X_CONTROLLER = new PIDController(0.0, 0.0, 0.0); + public static final PIDController Y_CONTROLLER = new PIDController(0.0, 0.0, 0.0); + public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController( + 1.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2 * Math.PI, Math.PI)); + // swerve configuration public static final double NEUTRAL_DEADBAND = 0.04; public static final double OPEN_LOOP_RAMP_RATE = 0.2; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a26b704..2b6d085 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -5,6 +5,10 @@ package frc4388.robot; +import java.lang.reflect.Array; +import java.nio.file.FileSystem; +import java.util.Arrays; +import java.util.HashMap; import java.util.List; import com.pathplanner.lib.PathPlanner; @@ -22,6 +26,7 @@ import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; +import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -111,12 +116,30 @@ public class RobotContainer { .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); } - public void configAuto(boolean pathplanner) { - - } + public SequentialCommandGroup runAuto(String path, double maxVel, double maxAccel) { + PathPlannerTrajectory traj = PathPlanner.loadPath(path, maxVel, maxAccel); - public void configAuto() { - configAuto(true); + PIDController xController = SwerveDriveConstants.X_CONTROLLER; + PIDController yController = SwerveDriveConstants.Y_CONTROLLER; + ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER; + thetaController.enableContinuousInput(-Math.PI, Math.PI); + + PPSwerveControllerCommand ppSCC = new PPSwerveControllerCommand( + traj, + m_robotSwerveDrive::getOdometry, + m_robotSwerveDrive.m_kinematics, + xController, + yController, + thetaController, + m_robotSwerveDrive::setModuleStates, + m_robotSwerveDrive); + + return new SequentialCommandGroup( + new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()), + new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(traj.getInitialPose())), + ppSCC, + new InstantCommand(() -> m_robotSwerveDrive.stopModules()) + ); } /** @@ -125,68 +148,37 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - // https://github.com/mjansen4857/pathplanner/wiki <-- Pathplanner Wiki + // https://github.com/mjansen4857/pathplanner/wiki <-- Pathplanner Wik - TrajectoryConfig config = new TrajectoryConfig(1.0, 1.0) - .setKinematics(m_robotSwerveDrive.m_kinematics); - Trajectory exTraj = TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(0)), - List.of(new Translation2d(0, 0)), - new Pose2d(1, 0, new Rotation2d(0)), - config); + // PathPlannerTrajectory ppfirstTestPath = PathPlanner.loadPath("First Test Path", 4.0, 4.0); + // PathPlannerTrajectory ppMoveForward = PathPlanner.loadPath("Move Forward", 1.0, 1.0); + // PathPlannerTrajectory ppRotate = PathPlanner.loadPath("Rotate", 1.0, 1.0); - Trajectory firstTestPath = PathPlanner.loadPath("First Test Path", 1.0, 1.0); - Trajectory moveForward = PathPlanner.loadPath("Move Forward", 1.0, 1.0); - Trajectory rotate = PathPlanner.loadPath("Rotate", 1.0, 1.0); + // PathPlannerTrajectory ppCurrent = PathPlanner.loadPath("First Test Path", 1.0, 1.0); // change this to change auto - Trajectory currentPath = moveForward; // change this to change auto + // PIDController xController = SwerveDriveConstants.X_CONTROLLER; + // PIDController yController = SwerveDriveConstants.Y_CONTROLLER; + // ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER; + // thetaController.enableContinuousInput(-Math.PI, Math.PI); - PIDController xController = new PIDController(10.0, 0.0, 0.0); - PIDController yController = new PIDController(1.3, 0.0, 0.0); - ProfiledPIDController thetaController = new ProfiledPIDController( - 10.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2 * Math.PI, Math.PI)); - thetaController.enableContinuousInput(-Math.PI, Math.PI); - - SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand( - currentPath, - m_robotSwerveDrive::getOdometry, - m_robotSwerveDrive.m_kinematics, - xController, - yController, - thetaController, - m_robotSwerveDrive::setModuleStates, - m_robotSwerveDrive - ); - - PathPlannerTrajectory ppfirstTestPath = PathPlanner.loadPath("First Test Path", 4.0, 4.0); - PathPlannerTrajectory ppMoveForward = PathPlanner.loadPath("Move Forward", 1.0, 1.0); - PathPlannerTrajectory ppRotate = PathPlanner.loadPath("Rotate", 1.0, 1.0); - - PathPlannerTrajectory ppCurrentPath = ppfirstTestPath; // change this to change auto - - PPSwerveControllerCommand ppSwerveControllerCommand = new PPSwerveControllerCommand( - ppCurrentPath, - m_robotSwerveDrive::getOdometry, - m_robotSwerveDrive.m_kinematics, - xController, - yController, - thetaController, - m_robotSwerveDrive::setModuleStates, - m_robotSwerveDrive - ); - - // return new SequentialCommandGroup( - // new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(currentPath.getInitialPose())), - // swerveControllerCommand, - // new InstantCommand(() -> m_robotSwerveDrive.stopModules()) + // PPSwerveControllerCommand ppSwerveControllerCommand = new PPSwerveControllerCommand( + // ppCurrent, + // m_robotSwerveDrive::getOdometry, + // m_robotSwerveDrive.m_kinematics, + // xController, + // yController, + // thetaController, + // m_robotSwerveDrive::setModuleStates, + // m_robotSwerveDrive // ); - return new SequentialCommandGroup( - new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()), - new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(ppCurrentPath.getInitialPose())), - ppSwerveControllerCommand, - new InstantCommand(() -> m_robotSwerveDrive.stopModules()) - ); + // return new SequentialCommandGroup( + // new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()), + // new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(ppCurrent.getInitialPose())), + // ppSwerveControllerCommand, + // new InstantCommand(() -> m_robotSwerveDrive.stopModules()) + // ); + return runAuto("Move Forward", 5.0, 5.0); } /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 72ba884..eb64a3c 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -208,8 +208,10 @@ public class SwerveDrive extends SubsystemBase { } /** Updates the field relative position of the robot. */ + public void updateOdometry() { - m_poseEstimator.update( m_gyro.getRotation2d(), + Rotation2d offset = new Rotation2d(Math.PI/2); + m_poseEstimator.update( m_gyro.getRotation2d()/*.plus(offset)*/, modules[0].getState(), modules[1].getState(), modules[2].getState(),