Auto Chooser and Rotation PID

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)

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

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

src/main/java/com/orbit/frc/subsystems/swerve/SwerveConfig.java

@@ -71,17 +71,20 @@ 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; 
 
     // 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;

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,21 +377,15 @@ 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);
 		SmartDashboard.putNumber("Current velocity Drive Y", this.currentVelocity.getY());
 		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);
 	}

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<Command> 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!");
     }
 }