diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index efe227b..6aad04d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -47,18 +47,17 @@ public final class Constants { public static final class PIDConstants { public static final int SWERVE_SLOT_IDX = 0; public static final int SWERVE_PID_LOOP_IDX = 1; - public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SWERVE_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0); } public static final class AutoConstants { - 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(0.0, 0.0, 0.0, - new TrapezoidProfile.Constraints(0.0, 0.0) - ); + public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0); + public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0); + public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0); + public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune - public static final double PATH_MAX_VEL = -1; // TODO: find the actual value - public static final double PATH_MAX_ACC = -1; // TODO: find the actual value + public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value + public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value } public static final class Conversions { @@ -106,6 +105,7 @@ public final class Constants { } public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value + public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value // dimensions public static final double WIDTH = 18.5; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 731275b..e4e0b39 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,11 +7,32 @@ package frc4388.robot; +import java.util.ArrayList; +import java.util.List; + +import org.opencv.objdetect.HOGDescriptor; + +import edu.wpi.first.math.controller.HolonomicDriveController; +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.Trajectory; +import edu.wpi.first.math.trajectory.TrajectoryConfig; +import edu.wpi.first.math.trajectory.TrajectoryGenerator; +import edu.wpi.first.math.trajectory.Trajectory.State; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; +import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; +import frc4388.robot.Constants.SwerveDriveConstants.PIDConstants; import frc4388.robot.commands.AutoBalance; import frc4388.robot.commands.JoystickPlayback; import frc4388.robot.commands.JoystickRecorder; @@ -68,8 +89,9 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); - new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive)); + // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive)); + // // .onFalse() new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); @@ -94,7 +116,70 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - return new InstantCommand(); + + //Create Trajectory Settings + TrajectoryConfig trajectoryConfig = new TrajectoryConfig(SwerveDriveConstants.AutoConstants.PATH_MAX_VEL, + SwerveDriveConstants.AutoConstants.PATH_MAX_ACC).setKinematics(m_robotSwerveDrive.getKinematics()); + + // generate trajectory + Trajectory trajectory = TrajectoryGenerator.generateTrajectory( + + new Pose2d(0, 0, new Rotation2d(0)), + List.of( + new Translation2d(0, 1)), + new Pose2d(0, 2, Rotation2d.fromDegrees(0)), + + trajectoryConfig); + + ArrayList simplePath = new ArrayList(); + simplePath.add(new Pose2d(0, 0, new Rotation2d(0))); + simplePath.add(new Pose2d(0, -1, new Rotation2d(0))); + + // Trajectory trajectory = TrajectoryGenerator.generateTrajectory(simplePath, trajectoryConfig); + // Trajectory trajectory = TrajectoryGenerator.generateTrajectory( + // new Pose2d(0, 0, new Rotation2d(0), + // List.of(new Translation2d(0, 1)), + // new Pose2d(0, 2, new Rotation2d(0)), + // trajectoryConfig + // ); + + + // ArrayList states = new ArrayList(); + + + + //Defining PID Controller for tracking trajectory + PIDController xController = new PIDController(SwerveDriveConstants.AutoConstants.X_CONTROLLER.kP, 0, 0); + PIDController yController = new PIDController(SwerveDriveConstants.AutoConstants.Y_CONTROLLER.kP, 0, 0); + ProfiledPIDController thetaController = new ProfiledPIDController(AutoConstants.THETA_CONTROLLER.kP, + AutoConstants.THETA_CONTROLLER.kI, AutoConstants.THETA_CONTROLLER.kD, AutoConstants.THETA_CONSTRAINTS); + thetaController.enableContinuousInput(-Math.PI, Math.PI); + + HolonomicDriveController holoController = new HolonomicDriveController(xController, yController, thetaController); + + //Command to follow trajectory + // SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand( + // trajectory, + // m_robotSwerveDrive::getOdometry, + // m_robotSwerveDrive.getKinematics(), + // holoController, + // m_robotSwerveDrive::setModuleStates, + // m_robotSwerveDrive); + + SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand( + trajectory, + m_robotSwerveDrive::getPoseEstimator, + m_robotSwerveDrive.getKinematics(), + holoController, + m_robotSwerveDrive::setModuleStates, + m_robotSwerveDrive); + + //Init and wrap-up, return everything + return new SequentialCommandGroup( + new InstantCommand(() -> m_robotSwerveDrive.resetPoseEstimator(), m_robotSwerveDrive), + // new InstantCommand(() -> m_robotSwerveDrive.setOdometry(trajectory.getInitialPose())), + swerveControllerCommand, + new InstantCommand(() -> m_robotSwerveDrive.stopModules())); } public DeadbandedXboxController getDeadbandedDriverController() { diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 9d73542..0b13b6c 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -5,6 +5,7 @@ package frc4388.robot.subsystems; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -37,7 +38,9 @@ public class SwerveDrive extends SubsystemBase { private RobotGyro gyro; - private SwerveDriveOdometry odometry; + // private SwerveDriveOdometry odometry; + + private SwerveDrivePoseEstimator poseEstimator; public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default public Rotation2d rotTarget = new Rotation2d(); @@ -51,15 +54,28 @@ public class SwerveDrive extends SubsystemBase { this.rightBack = rightBack; this.gyro = gyro; - this.odometry = new SwerveDriveOdometry( + // this.odometry = new SwerveDriveOdometry( + // kinematics, + // gyro.getRotation2d(), + // new SwerveModulePosition[] { + // leftFront.getPosition(), + // rightFront.getPosition(), + // leftBack.getPosition(), + // rightBack.getPosition() + // }, + // getOdometry() + // ); + + this.poseEstimator = new SwerveDrivePoseEstimator( kinematics, - gyro.getRotation2d(), + gyro.getRotation2d(), new SwerveModulePosition[] { leftFront.getPosition(), rightFront.getPosition(), leftBack.getPosition(), rightBack.getPosition() - } + }, + new Pose2d(0,0, new Rotation2d(0)) ); this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; @@ -68,21 +84,15 @@ public class SwerveDrive extends SubsystemBase { public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { if (fieldRelative) { - if (rightStick.getNorm() > 0.1) { rotTarget = new Rotation2d(rightStick.getX(), -rightStick.getY()).minus(new Rotation2d(0, 1)); } double rot = rotTarget.minus(gyro.getRotation2d()).getRadians(); - // Use the left joystick to set speed. Apply a quadratic curve and the set max speed. Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); - // if (rightStick.getNorm() < .1) { - // rot = 0; - // } - // Convert field-relative speeds to robot-relative speeds. chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1)); @@ -112,15 +122,27 @@ public class SwerveDrive extends SubsystemBase { public void resetGyro() { gyro.reset(); - setOdometry(getOdometry()); + // setOdometry(getOdometry()); rotTarget = new Rotation2d(0); } /** * Updates the odometry of the SwerveDrive. */ - public void updateOdometry() { - odometry.update( + // public void updateOdometry() { + // odometry.update( + // gyro.getRotation2d(), + // new SwerveModulePosition[] { + // leftFront.getPosition(), + // rightFront.getPosition(), + // leftBack.getPosition(), + // rightBack.getPosition() + // } + // ); + // } + + public void updatePoseEstimator() { + poseEstimator.update( gyro.getRotation2d(), new SwerveModulePosition[] { leftFront.getPosition(), @@ -135,16 +157,33 @@ public class SwerveDrive extends SubsystemBase { * Gets the odometry of the SwerveDrive. * @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta). */ - public Pose2d getOdometry() { - return odometry.getPoseMeters(); + // public Pose2d getOdometry() { + // return odometry.getPoseMeters(); + // } + + public Pose2d getPoseEstimator() { + return poseEstimator.getEstimatedPosition(); } /** * Sets the odometry of the SwerveDrive. * @param pose Pose to set the odometry to. */ - public void setOdometry(Pose2d pose) { - odometry.resetPosition( + // public void setOdometry(Pose2d pose) { + // odometry.resetPosition( + // gyro.getRotation2d(), + // new SwerveModulePosition[] { + // leftFront.getPosition(), + // rightFront.getPosition(), + // leftBack.getPosition(), + // rightBack.getPosition() + // }, + // pose + // ); + // } + + public void setPoseEstimator(Pose2d pose) { + poseEstimator.resetPosition( gyro.getRotation2d(), new SwerveModulePosition[] { leftFront.getPosition(), @@ -156,6 +195,10 @@ public class SwerveDrive extends SubsystemBase { ); } + public void resetPoseEstimator() { + setPoseEstimator(new Pose2d()); + } + public void stopModules() { for (SwerveModule module : this.modules) { module.stop(); @@ -166,9 +209,9 @@ public class SwerveDrive extends SubsystemBase { * Resets the odometry of the SwerveDrive to 0. * *NOTE: If you reset your gyro, this method MUST be called with the new gyro angle and wheel encoder positions. */ - public void resetOdometry() { - setOdometry(new Pose2d()); - } + // public void resetOdometry() { + // setOdometry(new Pose2d()); + // } public SwerveDriveKinematics getKinematics() { return this.kinematics; @@ -177,14 +220,17 @@ public class SwerveDrive extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run - updateOdometry(); - SmartDashboard.putNumber("Odo X (ft)", Units.metersToFeet(this.getOdometry().getX())); - SmartDashboard.putNumber("Odo Y (ft)", Units.metersToFeet(this.getOdometry().getY())); - SmartDashboard.putNumber("Odo Theta", this.getOdometry().getRotation().getDegrees()); + // updateOdometry(); + updatePoseEstimator(); + + // SmartDashboard.putNumber("Odo X (ft)", Units.metersToFeet(this.getOdometry().getX())); + // SmartDashboard.putNumber("Odo Y (ft)", Units.metersToFeet(this.getOdometry().getY())); + // SmartDashboard.putNumber("Odo Theta", this.getOdometry().getRotation().getDegrees()); + + // SmartDashboard.putNumber("Gyro Angle", getGyroAngle()); + // SmartDashboard.putNumber("rotTarget", this.rotTarget.getDegrees()); - SmartDashboard.putNumber("Gyro Angle", getGyroAngle()); - SmartDashboard.putNumber("rotTarget", this.rotTarget.getDegrees()); } /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 2616b71..a88fca7 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -132,13 +132,13 @@ public class SwerveModule extends SubsystemBase { SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); // calculate the difference between our current rotational position and our new rotational position - Rotation2d rotationDelta = state.angle.minus(currentRotation); // ? might need to be negative + Rotation2d rotationDelta = state.angle.minus(currentRotation); // calculate the new absolute position of the SwerveModule based on the difference in rotation double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION; // convert the CANCoder from its position reading to ticks - double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); // ? why feedback coefficient + double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); if (!ignoreAngle) { angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); diff --git a/src/main/java/frc4388/utility/AprilTag.java b/src/main/java/frc4388/utility/AprilTag.java new file mode 100644 index 0000000..c2ad269 --- /dev/null +++ b/src/main/java/frc4388/utility/AprilTag.java @@ -0,0 +1,13 @@ +package frc4388.utility; + +// This is a seperate class in case I want to encode rotation or other +// information about the tag +public class AprilTag { + public final double x, y, z; + + public AprilTag(double _x, double _y, double _z) { + x = _x; + y = _y; + z = _z; + } +} diff --git a/src/main/java/frc4388/utility/Gains.java b/src/main/java/frc4388/utility/Gains.java index 7cda1e0..7a3a026 100644 --- a/src/main/java/frc4388/utility/Gains.java +++ b/src/main/java/frc4388/utility/Gains.java @@ -54,6 +54,12 @@ public class Gains { this.kMinOutput = -1.0; } + public Gains(double kP, double kI, double kD) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + } + /** * Creates Gains object for PIDs * @param kP The P value.