changes for driving + auto-align

team20 · Feb 8, 2025 · e25552b · e25552b
1 parent 4dc869d
commit e25552b
Show file tree

Hide file tree

Showing 11 changed files with 729 additions and 644 deletions.
diff --git a/src/main/java/frc/robot/CommandComposer.java b/src/main/java/frc/robot/CommandComposer.java
@@ -0,0 +1,66 @@
+package frc.robot;
+
+import static edu.wpi.first.wpilibj2.command.Commands.*;
+import static frc.robot.Constants.DriveConstants.*;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.PoseEstimationSubsystem;
+
+public class CommandComposer {
+	private static DriveSubsystem m_driveSubsystem;
+	private static PoseEstimationSubsystem m_poseEstimationSubsystem;
+
+	public static void setSubsystems(DriveSubsystem driveSubsystem,
+			PoseEstimationSubsystem poseEstimationSubsystem) {
+		m_driveSubsystem = driveSubsystem;
+		m_poseEstimationSubsystem = poseEstimationSubsystem;
+	}
+
+	/**
+	 * Returns a {@code Command} for testing the rotation capability of the robot.
+	 * 
+	 * @return a {@code Command} for testing the rotation capability of the robot
+	 */
+	public static Command testRotation() {
+		double rotionalSpeed = kTurnMaxAngularSpeed * 0.9;
+		double duration = 2.0;
+		return sequence(
+				m_driveSubsystem.run(() -> m_driveSubsystem.setModuleAngles(0)).withTimeout(1),
+				m_driveSubsystem.run(() -> m_driveSubsystem.drive(.5, 0, rotionalSpeed, true))
+						.withTimeout(duration),
+				m_driveSubsystem.run(() -> m_driveSubsystem.drive(-.5, 0, -rotionalSpeed, true))
+						.withTimeout(duration),
+				m_driveSubsystem.run(() -> m_driveSubsystem.drive(0, 0, rotionalSpeed, false))
+						.withTimeout(duration),
+				m_driveSubsystem.run(() -> m_driveSubsystem.setModuleAngles(0)).withTimeout(1),
+				m_driveSubsystem.run(() -> m_driveSubsystem.drive(0, 0, -rotionalSpeed, false))
+						.withTimeout(duration),
+				m_driveSubsystem.run(() -> m_driveSubsystem.setModuleAngles(0)).withTimeout(1),
+				m_driveSubsystem.run(() -> m_driveSubsystem.drive(0, 0, rotionalSpeed, false))
+						.withTimeout(duration),
+				m_driveSubsystem.run(() -> m_driveSubsystem.setModuleAngles(0)).withTimeout(1),
+				m_driveSubsystem.run(() -> m_driveSubsystem.drive(0, 0, -rotionalSpeed, false))
+						.withTimeout(duration));
+	}
+
+	/**
+	 * Returns a {@code Command} for turning the robot toward the specified
+	 * {@code AprilTag}.
+	 * 
+	 * @param tagID the ID of the {@code AprilTag}
+	 * @return a {@code Command} for turning the robot toward the specified
+	 *         {@code AprilTag}
+	 */
+	public static Command turnTowardTag(int tagID) {
+		return m_driveSubsystem.run(() -> {
+			Rotation2d a = m_poseEstimationSubsystem.angularDisplacement(1);
+			if (a != null)
+				m_driveSubsystem.drive(
+						0, 0, a.getDegrees() * kTurnP,
+						false);
+		}).withTimeout(1);
+	}
+
+}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -12,17 +12,32 @@
 import edu.wpi.first.math.util.Units;
 
 public class Constants {
-
-	/**
-	 * The {@code AprilTagFieldLayout}.
-	 */
-	public static AprilTagFieldLayout kFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape);
-
 	public static final class ControllerConstants {
 		public static final int kDriverControllerPort = 0;
 		public static final int kOperatorControllerPort = 1;
 		public static final double kDeadzone = 0.05;
 		public static final double kTriggerDeadzone = .05;
+
+		public static final class Button {
+			/** Left middle button */
+			public static final int kSquare = 1;
+			/** Bottom button */
+			public static final int kX = 2;
+			/** Right middle button */
+			public static final int kCircle = 3;
+			/** Top button */
+			public static final int kTriangle = 4;
+			public static final int kLeftBumper = 5;
+			public static final int kRightBumper = 6;
+			public static final int kLeftTrigger = 7;
+			public static final int kRightTrigger = 8;
+			public static final int kShare = 9;
+			public static final int kOptions = 10;
+			public static final int kLeftStick = 11;
+			public static final int kRightStick = 12;
+			public static final int kPS = 13;
+			public static final int kTrackpad = 14;
+		}
 	}
 
 	public static final class DriveConstants {
@@ -48,13 +63,19 @@ public static final class DriveConstants {
 		public static final double kV = 0.11;
 		public static final double kA = 0.009;
 
-		public static final double kTeleopMaxVoltage = 12;
-		public static final double kTeleopMaxTurnVoltage = 7.2;
-		public static final double kGearRatio = 6.12;
+		// 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 kDriveMinSpeed = .3; // .3 meters per second // TODO: find a good value
+		public static final double kTurnMaxAngularSpeed = Math.toRadians(180); // 1/2 rotation per second
+		public static final double kTurnMinAngularSpeed = Math.toRadians(5); // 5 degrees per second
+		public static final double kDriveMaxVoltage = 12;
+
+		public static final double kDriveGearRatio = 6.12;
+		public static final double kSteerGearRatio = 150.0 / 7;
 		public static final double kWheelDiameter = Units.inchesToMeters(4);
 		public static final double kWheelCircumference = Math.PI * kWheelDiameter;
 
-		public static final double kMetersPerMotorRotation = kWheelCircumference / kGearRatio;
+		public static final double kMetersPerMotorRotation = kWheelCircumference / kDriveGearRatio;
 
 		// https://docs.wpilib.org/en/latest/docs/software/basic-programming/coordinate-system.html
 		public static final Translation2d kFrontLeftLocation = new Translation2d(0.381, 0.381);
@@ -64,7 +85,9 @@ public static final class DriveConstants {
 
 		public static final int kEncoderDepth = 4;
 		public static final int kEncoderMeasurementPeriod = 16;
-		public static final int kSteerSmartCurrentLimit = 60;
+		public static final int kDriveSmartCurrentLimit = 60; // TODO: find a good value
+		public static final int kDrivePeakCurrentLimit = kDriveSmartCurrentLimit + 15;
+		public static final int kSteerSmartCurrentLimit = 60; // TODO: find a good value
 		public static final int kSteerPeakCurrentLimit = kSteerSmartCurrentLimit + 15;
 		// The amount of time to go from 0 to full power in seconds
 		public static final double kRampRate = .1;
@@ -80,37 +103,42 @@ public static final class DriveConstants {
 			kDriveConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = kRampRate;
 		}
 
-		public static final double kModuleResponseTimeSeconds = 0.02;
-
 		// DriveCommand.java Constants
-		public static final double kDriveP = 4; // up to 1.0?
-		// public static final double kDriveP = 0.4; // up to 1.0?
+		public static final double kDriveP = 5; // TODO: find a good value
 		public static final double kDriveI = 0;
 		public static final double kDriveD = 0;
-		public static final double kDriveMaxVelocity = 3; // up to 5?
-		public static final double kDriveMaxAcceleration = 3; // up to 10?
+		// 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?
 		public static final double kTurnI = 0; // was 0.003
 		public static final double kTurnD = 0; // 0.0
-		public static final double kTurnMaxVelocity = 120; // up to 240?
-		public static final double kTurnMaxAcceleration = 240; // up to 360?
+		public static final double kTurnMaxAcceleration = 2 * kTurnMaxAngularSpeed; // kTurnMaxAngularSpeed in 1/2 sec
 
 	}
 
-	public static final class PoseConstants {
-		public static Translation2d kBlueReefPosition = new Translation2d(4.49, 4.03);
-		public static Translation2d kRedReefPosition = new Translation2d(13.06, 4.03);
-	}
+	/**
+	 * The {@code AprilTagFieldLayout}.
+	 */
+	public static AprilTagFieldLayout kFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape);
 
 	public static final class RobotConstants {
 
 		/**
-		 * The {@code Transform3d} expressing the pose of the camera relative to the
-		 * pose of the robot.
+		 * The {@code Transform3d} expressing the pose of the first camera relative to
+		 * the pose of the robot.
 		 */
-		public static Transform3d kRobotToCamera = new Transform3d(new Translation3d(0.3, 0.0, 0.2),
-				new Rotation3d(0, Units.degreesToRadians(-20), 0));
+		public static Transform3d kRobotToCamera1 = new Transform3d(new Translation3d(0.0, 0.0, 0.2),
+				new Rotation3d(0, Units.degreesToRadians(-10), 0));
+
+		/**
+		 * The {@code Transform3d} expressing the pose of the second camera relative to
+		 * the pose of the robot.
+		 */
+		public static Transform3d kRobotToCamera2 = new Transform3d(new Translation3d(-0.5, -0.0, 0.2),
+				new Rotation3d(0, Units.degreesToRadians(-20), Units.degreesToRadians(180)));
+
 	}
-}
+}