diff --git a/README.md b/README.md index 842cb28..6dd9485 100644 --- a/README.md +++ b/README.md @@ -17,3 +17,6 @@ Planned features: - Inverse kinematics for chained pivots and telescopes - Simulation support - Physics/kinematics perhaps? + +Note: +If on Windows, recommended to install "Testing tools core" from Visual Studio Build Tools C++ (for Google Tests run on this library) \ No newline at end of file diff --git a/build.gradle b/build.gradle index 096837d..4abb72b 100644 --- a/build.gradle +++ b/build.gradle @@ -11,18 +11,18 @@ plugins { repositories { mavenCentral() + maven { + url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit") + credentials { + username = "Mechanical-Advantage-Bot" + password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051" + } + } + mavenLocal() maven { name "JitPack" url "https://jitpack.io/" } - maven { - url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit") - credentials { - username = "Mechanical-Advantage-Bot" - password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051" - } - } - mavenLocal() } if (project.hasProperty('releaseMode')) { wpilibRepositories.addAllReleaseRepositories(project) @@ -57,8 +57,6 @@ dependencies { def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" - - } // Set up exports properly diff --git a/src/main/java/com/orbit/frc/commands/swerve/AlignToPose.java b/src/main/java/com/orbit/frc/commands/swerve/AlignToPose.java index d9dce0f..f4f6bf5 100644 --- a/src/main/java/com/orbit/frc/commands/swerve/AlignToPose.java +++ b/src/main/java/com/orbit/frc/commands/swerve/AlignToPose.java @@ -15,10 +15,23 @@ public class AlignToPose extends Command { private boolean allowEnd; + /** Creates a new AlignToPose Command + * Uses motion profiling for the translation of the swerve, and PID control for rotation based on ROBOT_ROTATION_PID in SwerveConfig + * @param swerveSubsystem instance of swerve subsystem created + * @param target the desired position to send robot + */ public AlignToPose(SwerveSubsystem swerveSubsystem, Pose2d target) { - this(swerveSubsystem, target, false); + this(swerveSubsystem, target, true); } + /** Creates a new AlignToPose Command + * Uses motion profiling for the translation of the swerve, and PID control for rotation based on ROBOT_ROTATION_PID in SwerveConfig + * @param swerveSubsystem instance of swerve subsystem created + * @param target the desired position to send robot + * @param allowEnd if command can end; in some cases, as this command requires swerve, not allowing end prevents driver from shifting bot manually + * The command will end if allowed to end (default is allowed), and if within provided tolerance (translation and rotation wise). + * Provide values in SwerveConfig + */ public AlignToPose(SwerveSubsystem swerveSubsystem, Pose2d target, boolean allowEnd) { this.swerveSubsystem = swerveSubsystem; this.target = target; @@ -42,9 +55,9 @@ public void execute() { @Override public boolean isFinished() { return allowEnd && (this.swerveSubsystem.drivePIDAtTarget() - || (Math.abs(this.swerveSubsystem.calculateDistanceToTarget(this.target)) < 0.05 + || (Math.abs(this.swerveSubsystem.calculateDistanceToTarget(this.target)) < this.swerveSubsystem.swerveConfig.positionToleranceMeters && Math.abs(this.target.getRotation().getDegrees() - - this.swerveSubsystem.currentPose().getRotation().getDegrees()) < 5.0)); + - this.swerveSubsystem.currentPose().getRotation().getDegrees()) < this.swerveSubsystem.swerveConfig.angleToleranceDegrees)); } @Override diff --git a/src/main/java/com/orbit/frc/subsystems/swerve/SwerveConfig.java b/src/main/java/com/orbit/frc/subsystems/swerve/SwerveConfig.java index b4fbf0a..a4cdd71 100644 --- a/src/main/java/com/orbit/frc/subsystems/swerve/SwerveConfig.java +++ b/src/main/java/com/orbit/frc/subsystems/swerve/SwerveConfig.java @@ -71,6 +71,9 @@ public class SwerveConfig { */ public SimpleMotorFeedforward DRIVE_SVA; + /** PID that controls the alignment of the robot (rotation wise, to a target) */ + public PIDConstants ROBOT_ROTATION_PID; + /** MAX speed for robot travel */ public double MAX_SPEED; public double MAX_ACCELERATION; @@ -78,10 +81,10 @@ public class SwerveConfig { // Auto Constants public com.pathplanner.lib.util.PIDConstants translation; public com.pathplanner.lib.util.PIDConstants rotation; - public double maxAngularVelocity; - public double maxAngularAcceleration; - public double positionTolerance; - public double angleTolerance; + public double maxAngularVelocityRadians; + public double maxAngularAccelerationRadians; + public double positionToleranceMeters; + public double angleToleranceDegrees; /** @@ -119,14 +122,14 @@ public static SwerveSubsystem createSwerve(SwerveConfig config, VisionConfig vis default: break; } + } - if (config.swerveDriveKinematics == null) { + if (config.swerveDriveKinematics == null) { config.swerveDriveKinematics = new SwerveDriveKinematics( new Translation2d(config.WHEEL_BASE_METERS / 2.0, config.TRACK_WIDTH_METERS / 2.0), new Translation2d(config.WHEEL_BASE_METERS / 2.0, -config.TRACK_WIDTH_METERS / 2.0), new Translation2d(-config.WHEEL_BASE_METERS / 2.0, config.TRACK_WIDTH_METERS / 2.0), new Translation2d(-config.WHEEL_BASE_METERS / 2.0, -config.TRACK_WIDTH_METERS / 2.0)); - } } config.DRIVE_CONVERSION_POSITION_FACTOR = (config.WHEEL_DIAMETER_METERS * Math.PI) / config.DRIVE_GEAR_RATIO; config.DRIVE_CONVERSION_VELOCITY_FACTOR = config.DRIVE_CONVERSION_POSITION_FACTOR / 60.0; diff --git a/src/main/java/com/orbit/frc/subsystems/swerve/SwerveSubsystem.java b/src/main/java/com/orbit/frc/subsystems/swerve/SwerveSubsystem.java index 670f7bf..172bae4 100644 --- a/src/main/java/com/orbit/frc/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/com/orbit/frc/subsystems/swerve/SwerveSubsystem.java @@ -49,11 +49,7 @@ public class SwerveSubsystem extends SubsystemBase { // Constants.Swerve.driveAlignPID.i, Constants.Swerve.driveAlignPID.d private double YkI = 0.00000; private double YkD = 0.000; - private double AkP = 0.0035; // changed mar 27 for testing 0.04; // 0.4? // A as in Angle for anglePID || - // replaces onstants.Swerve.anglePID.p, - // Constants.Swerve.anglePID.i, Constants.Swerve.anglePID.d - private double AkI = 0.000000; - private double AkD = 0.001; // 0.01? + public boolean manualDrive = false; private PhotonCameraWrapper pCameraWrapper; @@ -62,7 +58,7 @@ public class SwerveSubsystem extends SubsystemBase { private PIDController driveYPID = new PIDController(YkP, YkI, YkD); - private PIDController anglePID = new PIDController(AkP, AkI, AkD); + private PIDController anglePID; private TrapezoidProfile.Constraints driveConstraints; private TrapezoidProfile driveXMotionProfile; @@ -74,16 +70,11 @@ public class SwerveSubsystem extends SubsystemBase { private TrapezoidProfile.State driveMotionProfileYStartState; private TrapezoidProfile.State driveMotionProfileYEndState; - private TrapezoidProfile.State rotMotionProfileStartState; - private TrapezoidProfile.State rotMotionProfileEndState; - - private TrapezoidProfile rotMotionProfile; - private OrbitTimer timer; private boolean motionProfileInit = false; - private SwerveConfig swerveConfig; + public SwerveConfig swerveConfig; @AutoLogOutput(key = "Swerve/CurrentAcceleration") public Translation2d currentAcceleration = new Translation2d(0, 0); @@ -115,15 +106,17 @@ protected SwerveSubsystem(SwerveConfig swerveConfig, VisionConfig visionConfig) // Configure the AutoBuilder that handles all the auto path following!! SwerveAutoConfig.configureAutoBuilder(this, swerveConfig); - Preferences.initDouble("Swerve DriveX kP", this.XkP); - Preferences.initDouble("Swerve DriveX kI", this.XkI); - Preferences.initDouble("Swerve DriveX kD", this.XkD); - Preferences.initDouble("Swerve DriveY kP", this.YkP); - Preferences.initDouble("Swerve DriveY kI", this.YkI); - Preferences.initDouble("Swerve DriveY kD", this.YkD); - Preferences.initDouble("Swerve Angle kP", this.AkP); - Preferences.initDouble("Swerve Angle kI", this.AkI); - Preferences.initDouble("Swerve Angle kD", this.AkD); + // Preferences.initDouble("Swerve DriveX kP", this.XkP); + // Preferences.initDouble("Swerve DriveX kI", this.XkI); + // Preferences.initDouble("Swerve DriveX kD", this.XkD); + // Preferences.initDouble("Swerve DriveY kP", this.YkP); + // Preferences.initDouble("Swerve DriveY kI", this.YkI); + // Preferences.initDouble("Swerve DriveY kD", this.YkD); + Preferences.initDouble("Swerve Angle kP", swerveConfig.ROBOT_ROTATION_PID.p); + Preferences.initDouble("Swerve Angle kI", swerveConfig.ROBOT_ROTATION_PID.i); + Preferences.initDouble("Swerve Angle kD", swerveConfig.ROBOT_ROTATION_PID.d); + + this.anglePID = new PIDController(swerveConfig.ROBOT_ROTATION_PID.p, swerveConfig.ROBOT_ROTATION_PID.i, swerveConfig.ROBOT_ROTATION_PID.d); this.driveConstraints = new TrapezoidProfile.Constraints(swerveConfig.MAX_SPEED, swerveConfig.MAX_ACCELERATION); this.driveXMotionProfile = new TrapezoidProfile(driveConstraints); @@ -134,12 +127,6 @@ protected SwerveSubsystem(SwerveConfig swerveConfig, VisionConfig visionConfig) this.driveMotionProfileYStartState = new TrapezoidProfile.State(0.0, 0.0); this.driveMotionProfileYEndState = new TrapezoidProfile.State(0.0, 0.0); - this.rotMotionProfile = new TrapezoidProfile(new TrapezoidProfile.Constraints(45.0, 10.0)); // in degrees/sec - // and deg/s^2 - - this.rotMotionProfileStartState = new TrapezoidProfile.State(0.0, 0.0); - this.rotMotionProfileEndState = new TrapezoidProfile.State(0.0, 0.0); - this.timer = new OrbitTimer(); } @@ -374,14 +361,6 @@ public PIDSwerveValues calculateControlLoopDriveOutput(Pose2d target) { this.currentVelocity.getY()); this.driveMotionProfileYEndState = new TrapezoidProfile.State(target.getY(), 0.0); - Angles convertScopeResult = convertToAppropriateScope(target.getRotation().getDegrees()); - double currentAngle = convertScopeResult.currentAngle; - double targetAngle = convertScopeResult.targetAngle; - - this.rotMotionProfileStartState = new TrapezoidProfile.State(currentAngle, - this.currentVelocity.getRotation().getDegrees()); - this.rotMotionProfileEndState = new TrapezoidProfile.State(targetAngle, 0.0); - this.timer.start(); this.prevTargetPose = target; @@ -398,12 +377,8 @@ public PIDSwerveValues calculateControlLoopDriveOutput(Pose2d target) { this.driveMotionProfileYStartState, this.driveMotionProfileYEndState); - TrapezoidProfile.State rotProfileTarget = this.rotMotionProfile.calculate(this.timer.getTimeDeltaSec(), - this.rotMotionProfileStartState, this.rotMotionProfileEndState); - double profilePositionTargetX = profileTargetXPos.position; double profilePositionTargetY = profileTargetYPos.position; - double profilePositionTargetRot = rotProfileTarget.position; SmartDashboard.putNumber("Cur Pose Y", this.currentPose().getY()); SmartDashboard.putNumber("Profile Target Y Position", profilePositionTargetY); @@ -411,8 +386,6 @@ public PIDSwerveValues calculateControlLoopDriveOutput(Pose2d target) { SmartDashboard.putNumber("Target Profile Velocity Y", profileTargetYPos.velocity); SmartDashboard.putNumber("Current velocity Drive X", this.currentVelocity.getX()); SmartDashboard.putNumber("Target Profile Velocity X", profileTargetXPos.velocity); - SmartDashboard.putNumber("Current velocity Rot", this.currentVelocity.getRotation().getDegrees()); - SmartDashboard.putNumber("Target Profile Velocity Rot", rotProfileTarget.velocity); Pose2d currentPose = this.currentPose(); @@ -423,9 +396,6 @@ public PIDSwerveValues calculateControlLoopDriveOutput(Pose2d target) { double driveXOut = profileTargetXPos.velocity; double driveYOut = profileTargetYPos.velocity; - // double rotOut = -Math.toRadians(rotProfileTarget.velocity); // Drive method - // CW +, robot - // angles CCW + double rotOut = this.calculatePIDAngleOutput(target.getRotation().getDegrees()); @@ -439,9 +409,9 @@ public PIDSwerveValues calculateControlLoopDriveOutput(Pose2d target) { // double rotOut = // this.calculatePIDAngleOutput(target.getRotation().getDegrees()); // } - System.out.println("Current Pose: " + this.currentPose()); + //System.out.println("Current Pose: " + this.currentPose()); - System.out.println("Error X: " + driveXPID.getPositionError() + " Error Y: " + driveYPID.getPositionError()); + //System.out.println("Error X: " + driveXPID.getPositionError() + " Error Y: " + driveYPID.getPositionError()); // return new PIDSwerveValues(driveXOut, driveYOut, // this.calculatePIDAngleOutput(profilePositionTargetRot)); @@ -483,15 +453,15 @@ public void periodic() { lastPose = swerveDrivePoseEstimator.getEstimatedPosition(); lastPoseTimestamp = System.currentTimeMillis(); - Preferences.getDouble("Swerve DriveX kP", this.XkP); - Preferences.getDouble("Swerve DriveX kI", this.XkI); - Preferences.getDouble("Swerve DriveX kD", this.XkD); - Preferences.getDouble("Swerve DriveY kP", this.YkP); - Preferences.getDouble("Swerve DriveY kI", this.YkI); - Preferences.getDouble("Swerve DriveY kD", this.YkD); - Preferences.getDouble("Swerve Angle kP", this.AkP); - Preferences.getDouble("Swerve Angle kI", this.AkI); - Preferences.getDouble("Swerve Angle kD", this.AkD); + // Preferences.getDouble("Swerve DriveX kP", this.XkP); + // Preferences.getDouble("Swerve DriveX kI", this.XkI); + // Preferences.getDouble("Swerve DriveX kD", this.XkD); + // Preferences.getDouble("Swerve DriveY kP", this.YkP); + // Preferences.getDouble("Swerve DriveY kI", this.YkI); + // Preferences.getDouble("Swerve DriveY kD", this.YkD); + // Preferences.getDouble("Swerve Angle kP", this.AkP); + // Preferences.getDouble("Swerve Angle kI", this.AkI); + // Preferences.getDouble("Swerve Angle kD", this.AkD); SmartDashboard.putBoolean("Manual Drive Active", manualDrive); } diff --git a/src/main/java/com/orbit/frc/subsystems/swerve/autos/SwerveAutoConfig.java b/src/main/java/com/orbit/frc/subsystems/swerve/autos/SwerveAutoConfig.java index ddff7a7..4f99ef4 100644 --- a/src/main/java/com/orbit/frc/subsystems/swerve/autos/SwerveAutoConfig.java +++ b/src/main/java/com/orbit/frc/subsystems/swerve/autos/SwerveAutoConfig.java @@ -1,5 +1,6 @@ package com.orbit.frc.subsystems.swerve.autos; +import java.util.ArrayList; import java.util.Optional; import com.orbit.frc.subsystems.swerve.SwerveConfig; @@ -11,9 +12,18 @@ import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; public final class SwerveAutoConfig { + + private static SendableChooser autoChooser; + + public static void initalizeAutoChooser() { + autoChooser = AutoBuilder.buildAutoChooser(); + SmartDashboard.putData(autoChooser); + } public static void configureAutoBuilder(SwerveSubsystem swerve, SwerveConfig swerveConfig) { AutoBuilder.configureHolonomic( @@ -41,6 +51,7 @@ public static void configureAutoBuilder(SwerveSubsystem swerve, SwerveConfig swe swerve); Pathfinding.setPathfinder(new LocalADStar()); + initalizeAutoChooser(); System.out.println("AutoBuilder Configured!"); } }