tuned the parameters for the TestingBot

Author: jhhbrown1

src/main/java/frc/robot/Constants.java

@@ -56,7 +56,8 @@ public static final class DriveConstants {
 		// public static final double kP = 0.09;
 		public static final double kP = 0.03;
 		public static final double kI = 0.0;
-		public static final double kD = 0;
+		public static final double kD = kP * 0.05;
+		// public static final double kD = 0;
 		public static final double kS = 0;
 		// public static final double kV = 0.11;
 		// public static final double kA = 0.009;
@@ -68,7 +69,8 @@ public static final class DriveConstants {
 		// public static final double kTeleopMaxVoltage = 12;
 		// public static final double kTeleopMaxTurnVoltage = 7.2;
 
-		public static final double kDriveMaxSpeed = 3.0; // 3 meters per second
+		// public static final double kDriveMaxSpeed = 3.0; // 3 meters per second
+		public static final double kDriveMaxSpeed = 5.0; // 5 meters per second
 		public static final double kTurnMaxAngularSpeed = Math.PI; // 1/2 rotation per second
 		public static final double kDriveMaxVoltage = 12;
 
@@ -95,11 +97,13 @@ public static final class DriveConstants {
 		public static final double kRampRate = .1;
 
 		// DriveCommand.java Constants
-		public static final double kDriveP = 4; // up to 1.0?
+		public static final double kDriveP = 6; // up to 1.0?
 		// public static final double kDriveP = 0.4; // up to 1.0?
 		public static final double kDriveI = 0;
 		public static final double kDriveD = 0;
-		public static final double kDriveMaxAcceleration = 2 * kDriveMaxSpeed; // kDriveMaxSpeed in 1/2 sec
+		// public static final double kDriveMaxAcceleration = 2 * kDriveMaxSpeed; //
+		// kDriveMaxSpeed in 1/2 sec
+		public static final double kDriveMaxAcceleration = 1 * kDriveMaxSpeed; // kDriveMaxSpeed in 1 sec
 
 		public static final double kTurnP = 0.2; // was 0.005 upto 0.2?
 		// public static final double kTurnP = 0.02; // was 0.005 upto 0.2?
@@ -120,7 +124,7 @@ public static final class RobotConstants {
 		 * The {@code Transform3d} expressing the pose of the first camera relative to
 		 * the pose of the robot.
 		 */
-		public static Transform3d kRobotToCamera1 = new Transform3d(new Translation3d(0.0, -0.1, 0.2),
+		public static Transform3d kRobotToCamera1 = new Transform3d(new Translation3d(0.0, 0.0, 0.2),
 				new Rotation3d(0, Units.degreesToRadians(-20), 0));
 
 		/**

src/main/java/frc/robot/Robot.java

@@ -16,6 +16,7 @@
 import org.littletonrobotics.urcl.URCL;
 import org.photonvision.PhotonCamera;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.wpilibj.DataLogManager;
@@ -74,8 +75,8 @@ public void bindDriveControls() {
 						() -> m_driverController.getR2Axis() - m_driverController.getL2Axis(),
 						m_driverController.getHID()::getSquareButton));
 
-		double distanceTolerance = 0.05;
-		double angleTolerance = 5;
+		double distanceTolerance = 0.03;
+		double angleTolerance = 3;
 		m_driverController.button(Button.kSquare) // home
 				.whileTrue(
 						new DriveCommand(
@@ -90,36 +91,46 @@ public void bindDriveControls() {
 								DriveCommand.moveForward(m_driveSubsystem, -1.8, distanceTolerance, angleTolerance)));
 
 		Transform2d robotToTarget = new Transform2d(1, 0, Rotation2d.fromDegrees(180));
-		m_driverController.button(Button.kTriangle)
-				.whileTrue(
-						AlignCommand.moveTo(
-								m_driveSubsystem, m_poseEstimationSubystem,
-								kFieldLayout.getTagPose(17).get().toPose2d().plus(robotToTarget),
-								distanceTolerance, angleTolerance));
-
 		double angleOfCoverageInDegrees = 90;
 		double distanceThresholdInMeters = 4;
-		m_driverController.button(Button.kLeftBumper)
+		m_driverController.button(Button.kTriangle)
 				.whileTrue(
-						AlignCommand.turnToClosestTag(
-								m_driveSubsystem, m_poseEstimationSubystem, angleOfCoverageInDegrees,
-								distanceThresholdInMeters,
-								distanceTolerance, angleTolerance));
+						alignTest(1, robotToTarget, distanceTolerance, angleTolerance));
+		// m_driverController.button(Button.kTriangle)
+		// .whileTrue(
+		// AlignCommand.turnToClosestTag(
+		// m_driveSubsystem, m_poseEstimationSubystem, angleOfCoverageInDegrees,
+		// distanceThresholdInMeters,
+		// distanceTolerance, angleTolerance));
 
-		m_driverController.button(Button.kRightBumper)
+		m_driverController.button(Button.kLeftBumper)
 				.whileTrue(
 						AlignCommand.moveToClosestTag(
 								m_driveSubsystem, m_poseEstimationSubystem, angleOfCoverageInDegrees,
 								distanceThresholdInMeters, robotToTarget,
 								distanceTolerance, angleTolerance));
 
-		m_driverController.button(Button.kLeftTrigger)
+		m_driverController.button(Button.kRightBumper)
 				.whileTrue(
 						tourCommand(
-								m_driveSubsystem, m_poseEstimationSubystem, distanceTolerance, angleTolerance,
+								m_driveSubsystem, m_poseEstimationSubystem, distanceTolerance,
+								angleTolerance,
 								robotToTarget, 12, 15, 14, 16, 17, 18, 19, 20, 21, 22));
 	}
 
+	private Command alignTest(int tagIDs, Transform2d robotToTarget, double distanceTolerance, double angleTolerance) {
+		return sequence(
+				AlignCommand.moveTo(
+						m_driveSubsystem, m_poseEstimationSubystem,
+						kFieldLayout.getTagPose(1).get().toPose2d().plus(robotToTarget),
+						distanceTolerance, angleTolerance),
+				AlignCommand.moveTo(
+						m_driveSubsystem, m_poseEstimationSubystem,
+						() -> m_poseEstimationSubystem.getEstimatedPose()
+								.plus(pose(-2, 0, 0).minus(Pose2d.kZero)),
+						distanceTolerance, angleTolerance));
+	}
+
 	Command tourCommand(DriveSubsystem driveSubsystem, PoseEstimationSubsystem poseEstimationSubystem,
 			double distanceTolerance, double angleTolerance, Transform2d robotToTarget, int... tagIDs) {
 		List<Command> commands = Arrays.stream(tagIDs).mapToObj(i -> kFieldLayout.getTagPose(i))